pydrake.math¶
Bindings for math
, including overloads for scalar types and basic SE(3)
representations.
Note that arrays of symbolic scalar types, such as Variable
and
Expression
, are exposed using ndarray[object]
, and as such logical
operations are constrained to return boolean values given NumPy’s
implementation; this is not desirable, as one should really get a Formula
object. As a workaround, this module provides the following vectorized
operators, following suit with the operator
builtin module:
lt
, le
, eq
, ne
, ge
, and gt
.
As an example:
>>> x = np.array([Variable("x0"), Variable("x1")])
>>> y = np.array([Variable("y0"), Variable("y1")])
>>> x >= y
# This should throw a RuntimeError
>>> ge(x, y)
array([<Formula "(x0 >= y0)">, <Formula "(x1 >= y1)">], dtype=object)

pydrake.math.
abs
(*args, **kwargs)¶ Overloaded function.
 abs(arg0: float) > float
 abs(arg0: pydrake.autodiffutils.AutoDiffXd) > pydrake.autodiffutils.AutoDiffXd
 abs(arg0: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
acos
(*args, **kwargs)¶ Overloaded function.
 acos(arg0: float) > float
 acos(arg0: pydrake.autodiffutils.AutoDiffXd) > pydrake.autodiffutils.AutoDiffXd
 acos(arg0: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
asin
(*args, **kwargs)¶ Overloaded function.
 asin(arg0: float) > float
 asin(arg0: pydrake.autodiffutils.AutoDiffXd) > pydrake.autodiffutils.AutoDiffXd
 asin(arg0: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
atan
(*args, **kwargs)¶ Overloaded function.
 atan(arg0: float) > float
 atan(arg0: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
atan2
(*args, **kwargs)¶ Overloaded function.
 atan2(y: float, x: float) > float
 atan2(arg0: pydrake.autodiffutils.AutoDiffXd, arg1: pydrake.autodiffutils.AutoDiffXd) > pydrake.autodiffutils.AutoDiffXd
 atan2(arg0: pydrake.symbolic.Expression, arg1: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
BalanceQuadraticForms
(S: numpy.ndarray[float64[m, n], flags.f_contiguous], P: numpy.ndarray[float64[m, n], flags.f_contiguous]) → numpy.ndarray[float64[m, n]]¶ 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 modelorder 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 Snormal if T’ST = I and abs(T’PT) = D⁻², call it Pnormal 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=1e4 and st^2 = pt^2 = 1. But if s=10, p=1e7, then t=0.01, and st^2 = 1e3, 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.

class
pydrake.math.
BarycentricMesh
¶ Represents a multilinear function (from vector inputs to vector outputs) by interpolating between points on a mesh using (triangular) barycentric interpolation.
For a technical description of barycentric interpolation, see e.g. Remi Munos and Andrew Moore, “Barycentric Interpolators for Continuous Space and Time Reinforcement Learning”, NIPS 1998

__init__
(self: pydrake.math.BarycentricMesh, arg0: List[Set[float]]) → None¶ Constructs the mesh.

Eval
(self: pydrake.math.BarycentricMesh, arg0: numpy.ndarray[float64[m, n], flags.f_contiguous], arg1: numpy.ndarray[float64[m, 1]]) → numpy.ndarray[float64[m, 1]]¶ Returns the function evaluated at
input
.

EvalBarycentricWeights
(self: pydrake.math.BarycentricMesh, arg0: numpy.ndarray[float64[m, 1]]) → Tuple[numpy.ndarray[int32[m, 1]], numpy.ndarray[float64[m, 1]]]¶ Writes the mesh indices used for interpolation to
mesh_indices
, and the interpolating coefficients toweights
. Inputs that are outside the bounding box of the input_grid are interpolated as though they were projected (elementwise) to the closest face of the defined mesh. Parameter
input
:  must be a vector of length get_num_inputs().
 Parameter
mesh_indices
:  is a pointer to a vector of length get_num_interpolants().
 Parameter
weights
:  is a vector of coefficients (which sum to 1) of length get_num_interpolants().
 Parameter

get_all_mesh_points
(self: pydrake.math.BarycentricMesh) → numpy.ndarray[float64[m, n]]¶ Returns a matrix with all of the mesh points, one per column.

get_input_grid
(self: pydrake.math.BarycentricMesh) → List[Set[float]]¶

get_input_size
(self: pydrake.math.BarycentricMesh) → int¶

get_mesh_point
(self: pydrake.math.BarycentricMesh, arg0: int) → numpy.ndarray[float64[m, 1]]¶ Returns the position of a mesh point in the input space referenced by its scalar index to
point
. Parameter
index
:  must be in [0, get_num_mesh_points).
 Parameter

get_num_interpolants
(self: pydrake.math.BarycentricMesh) → int¶

get_num_mesh_points
(self: pydrake.math.BarycentricMesh) → int¶

MeshValuesFrom
(self: pydrake.math.BarycentricMesh, arg0: Callable[[numpy.ndarray[float64[m, 1]]], numpy.ndarray[float64[m, 1]]]) → numpy.ndarray[float64[m, n]]¶ Evaluates
vector_func
at all input mesh points and extracts the mesh value matrix that should be used to approximate the function with this barycentric interpolation.MatrixXd mesh_values = bary.MeshValuesFrom( [](const auto& x) { return Vector1d(std::sin(x[0])); });


pydrake.math.
ceil
(*args, **kwargs)¶ Overloaded function.
 ceil(arg0: float) > float
 ceil(arg0: pydrake.autodiffutils.AutoDiffXd) > float
 ceil(arg0: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
ComputeBasisFromAxis
(axis_index: int, axis_W: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 3]]¶ Creates a righthanded 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.
 Parameter
axis_index
:  The index of the axis (in the range [0,2]), with 0 corresponding to the xaxis, 1 corresponding to the yaxis, and zcorresponding to the zaxis.
 Parameter
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.
 Returns
R_WL
:  The computed basis.
Raises:  RuntimeError if the norm of
axis_W
is within 1e10 to zero or axis_index
does not lie in the range [0,2].
 Parameter

pydrake.math.
ContinuousAlgebraicRiccatiEquation
(A: numpy.ndarray[float64[m, n], flags.f_contiguous], B: numpy.ndarray[float64[m, n], flags.f_contiguous], Q: numpy.ndarray[float64[m, n], flags.f_contiguous], R: numpy.ndarray[float64[m, n], flags.f_contiguous]) → numpy.ndarray[float64[m, n]]¶ Computes the unique stabilizing solution S to the continuoustime algebraic Riccati equation:
\[S A + A' S  S B R^{1} B' S + Q = 0\]Raises: RuntimeError 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

pydrake.math.
cos
(*args, **kwargs)¶ Overloaded function.
 cos(arg0: float) > float
 cos(arg0: pydrake.autodiffutils.AutoDiffXd) > pydrake.autodiffutils.AutoDiffXd
 cos(arg0: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
cosh
(*args, **kwargs)¶ Overloaded function.
 cosh(arg0: float) > float
 cosh(arg0: pydrake.autodiffutils.AutoDiffXd) > pydrake.autodiffutils.AutoDiffXd
 cosh(arg0: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
DecomposePositiveQuadraticForm
(Q: numpy.ndarray[float64[m, n], flags.f_contiguous], b: numpy.ndarray[float64[m, 1]], c: float, tol: float = 0) → Tuple[numpy.ndarray[float64[m, n]], numpy.ndarray[float64[m, n]]]¶ 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.
 Parameter
Q
:  The square matrix.
 Parameter
b
:  The vector containing the linear coefficients.
 Parameter
c
:  The constatnt term.
 Parameter
tol
:  We will determine if this quadratic form is always nonnegative, by checking the Eigen values of the matrix [Q b/2] [bᵀ/2 c] are all greater than tol. $*Default:* is 0.
 Returns
(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 nonnegative, namely the matrix
[Q b/2] [bᵀ/2 c]
is positive semidefinite. 2.
Q
andb
are of the correct size. 3.tol
is nonnegative.Raises: RuntimeError if the precondition is not satisfied.  Parameter

pydrake.math.
DecomposePSDmatrixIntoXtransposeTimesX
(Y: numpy.ndarray[float64[m, n], flags.f_contiguous], zero_tol: float) → numpy.ndarray[float64[m, n]]¶ 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.
 Parameter
Y
:  A symmetric positive semidefinite matrix.
 Parameter
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.
 Returns
X
:  . The matrix X satisfies XᵀX = Y and X.rows() = rank(Y).
Precondition: 1. Y is positive semidefinite.
 zero_tol is nonnegative.
$Raises:
RuntimeError when the preconditions are not satisfied.
 Parameter

pydrake.math.
DiscreteAlgebraicRiccatiEquation
(A: numpy.ndarray[float64[m, n], flags.f_contiguous], B: numpy.ndarray[float64[m, n], flags.f_contiguous], Q: numpy.ndarray[float64[m, n], flags.f_contiguous], R: numpy.ndarray[float64[m, n], flags.f_contiguous]) → numpy.ndarray[float64[m, n]]¶ Computes the unique stabilizing solution X to the discretetime algebraic Riccati equation:
\[A'XA  X  A'XB(B'XB+R)^{1}B'XA + Q = 0\]Raises: RuntimeError if Q is not positive semidefinite. Raises: RuntimeError if R is not positive definite. Based on the Schur Vector approach outlined in this paper: “On the Numerical Solution of the DiscreteTime Algebraic Riccati Equation” by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell

pydrake.math.
exp
(*args, **kwargs)¶ Overloaded function.
 exp(arg0: float) > float
 exp(arg0: pydrake.autodiffutils.AutoDiffXd) > pydrake.autodiffutils.AutoDiffXd
 exp(arg0: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
floor
(*args, **kwargs)¶ Overloaded function.
 floor(arg0: float) > float
 floor(arg0: pydrake.autodiffutils.AutoDiffXd) > float
 floor(arg0: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
inv
(*args, **kwargs)¶ Overloaded function.
 inv(arg0: numpy.ndarray[float64[m, n]]) > numpy.ndarray[float64[m, n]]
 inv(arg0: numpy.ndarray[object[m, n]]) > numpy.ndarray[object[m, n]]
 inv(arg0: numpy.ndarray[object[m, n]]) > numpy.ndarray[object[m, n]]

pydrake.math.
IsPositiveDefinite
(matrix: numpy.ndarray[float64[m, n], flags.f_contiguous], tolerance: float = 0.0) → bool¶ Checks if a matrix is symmetric (with tolerance
symmetry_tolerance
–See also
IsSymmetric) and has all eigenvalues greater than
eigenvalue_tolerance
.eigenvalue_tolerance
must be >= 0 – where 0 implies positive semidefinite (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).

pydrake.math.
IsSymmetric
(*args, **kwargs)¶ Overloaded function.
 IsSymmetric(matrix: numpy.ndarray[float64[m, n], flags.f_contiguous]) > bool
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(matrix: numpy.ndarray[float64[m, n], flags.f_contiguous], precision: float) > bool
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.

pydrake.math.
log
(*args, **kwargs)¶ Overloaded function.
 log(arg0: float) > float
 log(arg0: pydrake.autodiffutils.AutoDiffXd) > pydrake.autodiffutils.AutoDiffXd
 log(arg0: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
max
(*args, **kwargs)¶ Overloaded function.
 max(arg0: float, arg1: float) > float
 max(arg0: pydrake.autodiffutils.AutoDiffXd, arg1: pydrake.autodiffutils.AutoDiffXd) > pydrake.autodiffutils.AutoDiffXd
 max(arg0: pydrake.symbolic.Expression, arg1: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
min
(*args, **kwargs)¶ Overloaded function.
 min(arg0: float, arg1: float) > float
 min(arg0: pydrake.autodiffutils.AutoDiffXd, arg1: pydrake.autodiffutils.AutoDiffXd) > pydrake.autodiffutils.AutoDiffXd
 min(arg0: pydrake.symbolic.Expression, arg1: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
pow
(*args, **kwargs)¶ Overloaded function.
 pow(arg0: float, arg1: float) > float
 pow(arg0: pydrake.autodiffutils.AutoDiffXd, arg1: float) > pydrake.autodiffutils.AutoDiffXd
 pow(arg0: pydrake.symbolic.Expression, arg1: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
RealContinuousLyapunovEquation
(A: numpy.ndarray[float64[m, n], flags.f_contiguous], Q: numpy.ndarray[float64[m, n], flags.f_contiguous]) → numpy.ndarray[float64[m, n]]¶  Parameter
A
:  A user defined real square matrix.
 Parameter
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.Raises:  RuntimeError 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 λⱼ.
Raises: RuntimeError 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 semidefinite, then X is also positive semidefinite [1]. Therefore, if one searches for a Lyapunov function V(z) = zᵀXz for the stable linear system ż = Az, then the solution of the Lyapunov Equation
AᵀX + XA + Q = 0
only returns a valid Lyapunov function if Q is positive semidefinite.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 2by2, then a Schur factorization is performed.
Raises: RuntimeError 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 1e10. It has been used to check (1) if λᵢ + λ̅ⱼ = 0, ∀ i,j; (2) if A is a 1by1 zero matrix; (3) if A’s trace or determinant is 0 when A is a 2by2 matrix.
[1] Bartels, R.H. and G.W. Stewart, “Solution of the Matrix Equation AX + XB = C,” Comm. of the ACM, Vol. 15, No. 9, 1972.
[2] http://slicot.org/objects/software/shared/doc/SB03MD.html
 Parameter

pydrake.math.
RealDiscreteLyapunovEquation
(A: numpy.ndarray[float64[m, n], flags.f_contiguous], Q: numpy.ndarray[float64[m, n], flags.f_contiguous]) → numpy.ndarray[float64[m, n]]¶  Parameter
A
:  A user defined real square matrix.
 Parameter
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.Raises:  RuntimeError 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].
Raises: RuntimeError 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 semidefinite, then X is also positive semidefinite [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 semidefinite.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 2by2, then a Schur factorization is performed.
Raises: RuntimeError 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 1e10. 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., AC22, pp. 883885, 1977.
[2] http://slicot.org/objects/software/shared/doc/SB03MD.html
 Parameter

pydrake.math.
RigidTransform
¶ alias of
pydrake.math.RigidTransform_[float]

template
pydrake.math.
RigidTransform_
¶ Instantiations:
RigidTransform_[float]
,RigidTransform_[AutoDiffXd]
,RigidTransform_[Expression]

class
RigidTransform_[float]
¶ This class represents a proper rigid transform between two frames which can be regarded in two ways. A rigid transform describes the “pose” between two frames A and B (i.e., the relative orientation and position of A to B). Alternately, it can be regarded as a distancepreserving operator that can rotate and/or translate a rigid body without changing its shape or size (rigid) and without mirroring/reflecting the body (proper), e.g., it can add one position vector to another and express the result in a particular basis as
p_AoQ_A = X_AB * p_BoQ_B
(Q is any point). In many ways, this rigid transform class is conceptually similar to using a homogeneous matrix as a linear operator. See operator* documentation for an exception.The class stores a RotationMatrix that relates righthanded orthogonal unit vectors Ax, Ay, Az fixed in frame A to righthanded orthogonal unit vectors Bx, By, Bz fixed in frame B. The class also stores a position vector from Ao (the origin of frame A) to Bo (the origin of frame B). The position vector is expressed in frame A. The monogram notation for the transform relating frame A to B is
X_AB
. The monogram notation for the rotation matrix relating A to B isR_AB
. The monogram notation for the position vector from Ao to Bo isp_AoBo_A
. See multibody_quantities for monogram notation for dynamics.Note
This class does not store the frames associated with the transform and cannot enforce correct usage of this class. For example, it makes sense to multiply RigidTransforms as
X_AB * X_BC
, but notX_AB * X_CB
.Note
This class is not a 4x4 transformation matrix – even though its operator*() methods act mostly like 4x4 matrix multiplication. Instead, this class contains a 3x3 rotation matrix class and a 3x1 position vector. To convert this to a 3x4 matrix, use GetAsMatrix34(). To convert this to a 4x4 matrix, use GetAsMatrix4(). To convert this to an Eigen::Isometry, use GetAsIsometry().
Note
An isometry is sometimes regarded as synonymous with rigid transform. The RigidTransform class has important advantages over Eigen::Isometry.  RigidTransform is built on an underlying rigorous 3x3 RotationMatrix class that has significant functionality for 3D orientation.  In Debug builds, RigidTransform requires a valid 3x3 rotation matrix and a valid (nonNAN) position vector. Eigen::Isometry does not.  RigidTransform catches bugs that are undetected by Eigen::Isometry.  RigidTransform has additional functionality and easeofuse, resulting in shorter, easier to write, and easier to read code.  The name Isometry is unfamiliar to many roboticists and dynamicists and for them Isometry.linear() is (for example) a counterintuitive method name to return a rotation matrix.
Note
One of the constructors in this class provides an implicit conversion from an Eigen Translation to RigidTransform.
 Authors:
 Paul Mitiguy (2018) Original author.
 Authors:
 Drake team (see https://drake.mit.edu/credits).

__init__
(*args, **kwargs)¶ Overloaded function.
 __init__(self: pydrake.math.RigidTransform_[float]) > None
Constructs the RigidTransform that corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence, the constructed RigidTransform contains an identity RotationMatrix and a zero position vector.
 __init__(self: pydrake.math.RigidTransform_[float], other: pydrake.math.RigidTransform_[float]) > None
 __init__(self: pydrake.math.RigidTransform_[float], R: drake::math::RotationMatrix<double>, p: numpy.ndarray[float64[3, 1]]) > None
Constructs a RigidTransform from a rotation matrix and a position vector.
 Parameter
R
:  rotation matrix relating frames A and B (e.g.,
R_AB
).  Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
 __init__(self: pydrake.math.RigidTransform_[float], rpy: drake::math::RollPitchYaw<double>, p: numpy.ndarray[float64[3, 1]]) > None
Constructs a RigidTransform from a RollPitchYaw and a position vector.
 Parameter
rpy
:  a RollPitchYaw which is a Spacefixed (extrinsic) XYZ rotation
with “rollpitchyaw” angles
[r, p, y]
or equivalently a Body fixed (intrinsic) ZYX rotation with “yawpitchroll” angles[y, p, r]
.
See also
RotationMatrix::RotationMatrix(const RollPitchYaw<T>&)
 Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
 __init__(self: pydrake.math.RigidTransform_[float], quaternion: Eigen::Quaternion<double, 0>, p: numpy.ndarray[float64[3, 1]]) > None
Constructs a RigidTransform from a Quaternion and a position vector.
 Parameter
quaternion
:  a nonzero, finite quaternion which may or may not have unit
length [i.e.,
quaternion.norm()
does not have to be 1].  Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
Raises:  RuntimeError in debug builds if the rotation matrix that is built
 from
quaternion
is invalid.
See also
RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&)
 __init__(self: pydrake.math.RigidTransform_[float], theta_lambda: Eigen::AngleAxis<double>, p: numpy.ndarray[float64[3, 1]]) > None
Constructs a RigidTransform from an AngleAxis and a position vector.
 Parameter
theta_lambda
:  an Eigen::AngleAxis whose associated axis (vector direction herein
called
lambda
) is nonzero and finite, but which may or may not have unit length [i.e.,lambda.norm()
does not have to be 1].  Parameter
p
:  position vector from frame A’s origin to frame B’s origin, expressed in frame A. In monogram notation p is denoted ``p_AoBo_A
Raises:  RuntimeError in debug builds if the rotation matrix that is built
 from theta_lambda` is invalid.
See also
RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&)
 __init__(self: pydrake.math.RigidTransform_[float], R: drake::math::RotationMatrix<double>) > None
Constructs a RigidTransform with a given RotationMatrix and a zero position vector.
 Parameter
R
:  rotation matrix relating frames A and B (e.g.,
R_AB
).
 __init__(self: pydrake.math.RigidTransform_[float], p: numpy.ndarray[float64[3, 1]]) > None
Constructs a RigidTransform that contains an identity RotationMatrix and a given position vector
p
. Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
 __init__(self: pydrake.math.RigidTransform_[float], pose: Eigen::Transform<double, 3, 1, 0>) > None
Constructs a RigidTransform from an Eigen Isometry3.
 Parameter
pose
:  Isometry3 that contains an allegedly valid rotation matrix
R_AB
and also contains a position vectorp_AoBo_A
from frame A’s origin to frame B’s origin.p_AoBo_A
must be expressed in frame A.
Raises:  RuntimeError in debug builds if R_AB is not a proper orthonormal
 3x3 rotation matrix.
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix(). __init__(self: pydrake.math.RigidTransform_[float], pose: numpy.ndarray[float64[m, n]]) > None
Constructs a RigidTransform from an appropriate Eigen expression.
 Parameter
pose
:  Generic Eigen matrix expression.
Raises:  RuntimeError if the Eigen expression in pose does not resolve
 to a Vector3 or 3x4 matrix or 4x4 matrix or if the rotational part
 of
pose
is not a proper orthonormal 3x3 rotation matrix or if pose
is a 4x4 matrix whose final row is not[0, 0, 0, 1]
.
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix().Note
This constructor prevents ambiguity that would otherwise exist for a RigidTransform constructor whose argument is an Eigen expression.
const Vector3<double> position(4, 5, 6); const RigidTransform<double> X1(3 * position);  const RotationMatrix<double> R(RollPitchYaw<double>(1, 2, 3)); Eigen::Matrix<double, 3, 4> pose34; pose34 << R.matrix(), position; const RigidTransform<double> X2(1.0 * pose34);  Eigen::Matrix<double, 4, 4> pose4; pose4 << R.matrix(), position, 0, 0, 0, 1; const RigidTransform<double> X3(pose4 * pose4);

template
cast
¶ Instantiations:
cast[float]
,cast[AutoDiffXd]
,cast[Expression]

cast[float]
(self: pydrake.math.RigidTransform_[float]) → pydrake.math.RigidTransform_[float]¶ Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T. For example,
RigidTransform<double> source = RigidTransform<double>::Identity(); RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RigidTransform is templated.
Note
RigidTransform<From>::cast<To>()
creates a newRigidTransform<To>
from aRigidTransform<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter


cast[AutoDiffXd]
(self: pydrake.math.RigidTransform_[float]) → drake::math::RigidTransform<Eigen::AutoDiffScalar<Eigen::Matrix<double, 1, 1, 0, 1, 1> > >¶ Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T. For example,
RigidTransform<double> source = RigidTransform<double>::Identity(); RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RigidTransform is templated.
Note
RigidTransform<From>::cast<To>()
creates a newRigidTransform<To>
from aRigidTransform<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter

cast[Expression]
(self: pydrake.math.RigidTransform_[float]) → drake::math::RigidTransform<drake::symbolic::Expression>¶ Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T. For example,
RigidTransform<double> source = RigidTransform<double>::Identity(); RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RigidTransform is templated.
Note
RigidTransform<From>::cast<To>()
creates a newRigidTransform<To>
from aRigidTransform<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter

cast[float]
(self: pydrake.math.RigidTransform_[float]) → pydrake.math.RigidTransform_[float]¶ Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T. For example,
RigidTransform<double> source = RigidTransform<double>::Identity(); RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RigidTransform is templated.
Note
RigidTransform<From>::cast<To>()
creates a newRigidTransform<To>
from aRigidTransform<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter

GetAsIsometry3
(self: pydrake.math.RigidTransform_[float]) → Eigen::Transform<double, 3, 1, 0>¶ Returns the isometry in ℜ³ that is equivalent to a RigidTransform.

GetAsMatrix34
(self: pydrake.math.RigidTransform_[float]) → numpy.ndarray[float64[3, 4]]¶ Returns the 3x4 matrix associated with this RigidTransform, i.e., X_AB.
┌ ┐ │ R_AB p_AoBo_A │ └ ┘

GetAsMatrix4
(self: pydrake.math.RigidTransform_[float]) → numpy.ndarray[float64[4, 4]]¶ Returns the 4x4 matrix associated with this RigidTransform, i.e., X_AB.
┌ ┐ │ R_AB p_AoBo_A │ │ │ │ 0 1 │ └ ┘

static
Identity
() → pydrake.math.RigidTransform_[float]¶ Returns the identity RigidTransform (corresponds to coincident frames).
Returns: the RigidTransform that corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence, the returned RigidTransform contains a 3x3 identity matrix and a zero position vector.

inverse
(self: pydrake.math.RigidTransform_[float]) → pydrake.math.RigidTransform_[float]¶ Returns X_BA = X_AB⁻¹, the inverse of
this
RigidTransform.Note
The inverse of RigidTransform X_AB is X_BA, which contains the rotation matrix R_BA = R_AB⁻¹ = R_ABᵀ and the position vector
p_BoAo_B_
(position from B’s origin Bo to A’s origin Ao, expressed in frame B).Note
: The squareroot of a RigidTransform’s condition number is roughly the magnitude of the position vector. The accuracy of the calculation for the inverse of a RigidTransform drops off with the sqrt condition number.

linear
(self: pydrake.math.RigidTransform_[float]) → numpy.ndarray[float64[3, 3]]¶ DO NOT USE! We offer this API for backwards compatibility with Isometry3, but it will be removed on or around 20200701. See drake issue #9865 for details.

matrix
(self: pydrake.math.RigidTransform_[float]) → numpy.ndarray[float64[4, 4]]¶ DO NOT USE! We offer this API for backwards compatibility with Isometry3, but it will be removed on or around 20200701. See drake issue #9865 for details.

multiply
(*args, **kwargs)¶ Overloaded function.
 multiply(self: pydrake.math.RigidTransform_[float], other: pydrake.math.RigidTransform_[float]) > pydrake.math.RigidTransform_[float]
Multiplies
this
RigidTransformX_AB
by theother
RigidTransformX_BC
and returns the RigidTransformX_AC = X_AB * X_BC
. multiply(self: pydrake.math.RigidTransform_[float], p_BoQ_B: numpy.ndarray[float64[3, 1]]) > numpy.ndarray[float64[3, 1]]
Multiplies
this
RigidTransformX_AB
by the position vectorp_BoQ_B
which is from Bo (B’s origin) to an arbitrary point Q. Parameter
p_BoQ_B
:  position vector from Bo to Q, expressed in frame B.
 Returns
p_AoQ_A
:  position vector from Ao to Q, expressed in frame A.
 multiply(self: pydrake.math.RigidTransform_[float], p_BoQ_B: numpy.ndarray[float64[3, n]]) > numpy.ndarray[float64[3, n]]
Multiplies
this
RigidTransformX_AB
by the n position vectorsp_BoQ1_B
… p_BoQn_B, wherep_BoQi_B
is the iᵗʰ position vector from Bo (frame B’s origin) to an arbitrary point Qi, expressed in frame B. Parameter
p_BoQ_B
: 3 x n
matrix with n position vectorsp_BoQi_B
or an expression that resolves to a3 x n
matrix of position vectors. Returns
p_AoQ_A
: 3 x n
matrix with n position vectorsp_AoQi_A
, i.e., n position vectors from Ao (frame A’s origin) to Qi, expressed in frame A. Specifically, this operator* is defined so thatX_AB * p_BoQ_B
returnsp_AoQ_A = p_AoBo_A + R_AB * p_BoQ_B
, wherep_AoBo_A
is the position vector from Ao to Bo expressed in A andR_AB
is the rotation matrix relating the orientation of frames A and B.
Note
As needed, use parentheses. This operator* is not associative. To see this, let
p = p_AoBo_A
, q = p_BoQ_B and note (X_AB * q) * 7 = (p + R_AB * q) * 7 ≠ X_AB * (q * 7) = p + R_AB * (q * 7).const RollPitchYaw<double> rpy(0.1, 0.2, 0.3); const RigidTransform<double> X_AB(rpy, Vector3d(1, 2, 3)); Eigen::Matrix<double, 3, 2> p_BoQ_B; p_BoQ_B.col(0) = Vector3d(4, 5, 6); p_BoQ_B.col(1) = Vector3d(9, 8, 7); const Eigen::Matrix<double, 3, 2> p_AoQ_A = X_AB * p_BoQ_B;

rotation
(self: pydrake.math.RigidTransform_[float]) → drake::math::RotationMatrix<double>¶ Returns R_AB, the rotation matrix portion of
this
RigidTransform. Returns
R_AB
:  the rotation matrix portion of
this
RigidTransform.
 Returns

set
(self: pydrake.math.RigidTransform_[float], R: drake::math::RotationMatrix<double>, p: numpy.ndarray[float64[3, 1]]) → None¶ Sets
this
RigidTransform from a RotationMatrix and a position vector. Parameter
R
:  rotation matrix relating frames A and B (e.g.,
R_AB
).  Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
 Parameter

set_rotation
(*args, **kwargs)¶ Overloaded function.
 set_rotation(self: pydrake.math.RigidTransform_[float], R: drake::math::RotationMatrix<double>) > None
Sets the RotationMatrix portion of
this
RigidTransform. Parameter
R
:  rotation matrix relating frames A and B (e.g.,
R_AB
).
 set_rotation(self: pydrake.math.RigidTransform_[float], rpy: drake::math::RollPitchYaw<double>) > None
Sets the rotation part of
this
RigidTransform from a RollPitchYaw. Parameter
rpy
:  “rollpitchyaw” angles.
See also
RotationMatrix::RotationMatrix(const RollPitchYaw<T>&) which describes the parameter, preconditions, etc.
 set_rotation(self: pydrake.math.RigidTransform_[float], quaternion: Eigen::Quaternion<double, 0>) > None
Sets the rotation part of
this
RigidTransform from a Quaternion. Parameter
quaternion
:  a quaternion which may or may not have unit length.
See also
RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&) which describes the parameter, preconditions, exception conditions, etc.
 set_rotation(self: pydrake.math.RigidTransform_[float], theta_lambda: Eigen::AngleAxis<double>) > None
Sets the rotation part of
this
RigidTransform from an AngleAxis. Parameter
theta_lambda
:  an angle
theta
(in radians) and vectorlambda
.
See also
RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&) which describes the parameter, preconditions, exception conditions, etc.

set_translation
(self: pydrake.math.RigidTransform_[float], p: numpy.ndarray[float64[3, 1]]) → None¶ Sets the position vector portion of
this
RigidTransform. Parameter
p
:  position vector from Ao (frame A’s origin) to Bo (frame B’s origin) expressed in frame A. In monogram notation p is denoted p_AoBo_A.
 Parameter

SetFromIsometry3
(self: pydrake.math.RigidTransform_[float], pose: Eigen::Transform<double, 3, 1, 0>) → None¶ Sets
this
RigidTransform from an Eigen Isometry3. Parameter
pose
:  Isometry3 that contains an allegedly valid rotation matrix
R_AB
and also contains a position vectorp_AoBo_A
from frame A’s origin to frame B’s origin.p_AoBo_A
must be expressed in frame A.
Raises:  RuntimeError in debug builds if R_AB is not a proper orthonormal
 3x3 rotation matrix.
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix(). Parameter

SetIdentity
(self: pydrake.math.RigidTransform_[float]) → pydrake.math.RigidTransform_[float]¶ Sets
this
RigidTransform so it corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence,this
RigidTransform contains a 3x3 identity matrix and a zero position vector.

translation
(self: pydrake.math.RigidTransform_[float]) → numpy.ndarray[float64[3, 1]]¶ Returns
p_AoBo_A
, the position vector portion ofthis
RigidTransform, i.e., position vector from Ao (frame A’s origin) to Bo (frame B’s origin).

class

class
pydrake.math.
RigidTransform_[AutoDiffXd]
¶ This class represents a proper rigid transform between two frames which can be regarded in two ways. A rigid transform describes the “pose” between two frames A and B (i.e., the relative orientation and position of A to B). Alternately, it can be regarded as a distancepreserving operator that can rotate and/or translate a rigid body without changing its shape or size (rigid) and without mirroring/reflecting the body (proper), e.g., it can add one position vector to another and express the result in a particular basis as
p_AoQ_A = X_AB * p_BoQ_B
(Q is any point). In many ways, this rigid transform class is conceptually similar to using a homogeneous matrix as a linear operator. See operator* documentation for an exception.The class stores a RotationMatrix that relates righthanded orthogonal unit vectors Ax, Ay, Az fixed in frame A to righthanded orthogonal unit vectors Bx, By, Bz fixed in frame B. The class also stores a position vector from Ao (the origin of frame A) to Bo (the origin of frame B). The position vector is expressed in frame A. The monogram notation for the transform relating frame A to B is
X_AB
. The monogram notation for the rotation matrix relating A to B isR_AB
. The monogram notation for the position vector from Ao to Bo isp_AoBo_A
. See multibody_quantities for monogram notation for dynamics.Note
This class does not store the frames associated with the transform and cannot enforce correct usage of this class. For example, it makes sense to multiply RigidTransforms as
X_AB * X_BC
, but notX_AB * X_CB
.Note
This class is not a 4x4 transformation matrix – even though its operator*() methods act mostly like 4x4 matrix multiplication. Instead, this class contains a 3x3 rotation matrix class and a 3x1 position vector. To convert this to a 3x4 matrix, use GetAsMatrix34(). To convert this to a 4x4 matrix, use GetAsMatrix4(). To convert this to an Eigen::Isometry, use GetAsIsometry().
Note
An isometry is sometimes regarded as synonymous with rigid transform. The RigidTransform class has important advantages over Eigen::Isometry.  RigidTransform is built on an underlying rigorous 3x3 RotationMatrix class that has significant functionality for 3D orientation.  In Debug builds, RigidTransform requires a valid 3x3 rotation matrix and a valid (nonNAN) position vector. Eigen::Isometry does not.  RigidTransform catches bugs that are undetected by Eigen::Isometry.  RigidTransform has additional functionality and easeofuse, resulting in shorter, easier to write, and easier to read code.  The name Isometry is unfamiliar to many roboticists and dynamicists and for them Isometry.linear() is (for example) a counterintuitive method name to return a rotation matrix.
Note
One of the constructors in this class provides an implicit conversion from an Eigen Translation to RigidTransform.
 Authors:
 Paul Mitiguy (2018) Original author.
 Authors:
 Drake team (see https://drake.mit.edu/credits).

__init__
(*args, **kwargs)¶ Overloaded function.
 __init__(self: pydrake.math.RigidTransform_[AutoDiffXd]) > None
Constructs the RigidTransform that corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence, the constructed RigidTransform contains an identity RotationMatrix and a zero position vector.
 __init__(self: pydrake.math.RigidTransform_[AutoDiffXd], other: pydrake.math.RigidTransform_[AutoDiffXd]) > None
 __init__(self: pydrake.math.RigidTransform_[AutoDiffXd], R: drake::math::RotationMatrix<Eigen::AutoDiffScalar<Eigen::Matrix<double, 1, 1, 0, 1, 1> > >, p: numpy.ndarray[object[3, 1]]) > None
Constructs a RigidTransform from a rotation matrix and a position vector.
 Parameter
R
:  rotation matrix relating frames A and B (e.g.,
R_AB
).  Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
 __init__(self: pydrake.math.RigidTransform_[AutoDiffXd], rpy: drake::math::RollPitchYaw<Eigen::AutoDiffScalar<Eigen::Matrix<double, 1, 1, 0, 1, 1> > >, p: numpy.ndarray[object[3, 1]]) > None
Constructs a RigidTransform from a RollPitchYaw and a position vector.
 Parameter
rpy
:  a RollPitchYaw which is a Spacefixed (extrinsic) XYZ rotation
with “rollpitchyaw” angles
[r, p, y]
or equivalently a Body fixed (intrinsic) ZYX rotation with “yawpitchroll” angles[y, p, r]
.
See also
RotationMatrix::RotationMatrix(const RollPitchYaw<T>&)
 Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
 __init__(self: pydrake.math.RigidTransform_[AutoDiffXd], quaternion: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd], p: numpy.ndarray[object[3, 1]]) > None
Constructs a RigidTransform from a Quaternion and a position vector.
 Parameter
quaternion
:  a nonzero, finite quaternion which may or may not have unit
length [i.e.,
quaternion.norm()
does not have to be 1].  Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
Raises:  RuntimeError in debug builds if the rotation matrix that is built
 from
quaternion
is invalid.
See also
RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&)
 __init__(self: pydrake.math.RigidTransform_[AutoDiffXd], theta_lambda: pydrake.common.eigen_geometry.AngleAxis_[AutoDiffXd], p: numpy.ndarray[object[3, 1]]) > None
Constructs a RigidTransform from an AngleAxis and a position vector.
 Parameter
theta_lambda
:  an Eigen::AngleAxis whose associated axis (vector direction herein
called
lambda
) is nonzero and finite, but which may or may not have unit length [i.e.,lambda.norm()
does not have to be 1].  Parameter
p
:  position vector from frame A’s origin to frame B’s origin, expressed in frame A. In monogram notation p is denoted ``p_AoBo_A
Raises:  RuntimeError in debug builds if the rotation matrix that is built
 from theta_lambda` is invalid.
See also
RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&)
 __init__(self: pydrake.math.RigidTransform_[AutoDiffXd], R: drake::math::RotationMatrix<Eigen::AutoDiffScalar<Eigen::Matrix<double, 1, 1, 0, 1, 1> > >) > None
Constructs a RigidTransform with a given RotationMatrix and a zero position vector.
 Parameter
R
:  rotation matrix relating frames A and B (e.g.,
R_AB
).
 __init__(self: pydrake.math.RigidTransform_[AutoDiffXd], p: numpy.ndarray[object[3, 1]]) > None
Constructs a RigidTransform that contains an identity RotationMatrix and a given position vector
p
. Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
 __init__(self: pydrake.math.RigidTransform_[AutoDiffXd], pose: pydrake.common.eigen_geometry.Isometry3_[AutoDiffXd]) > None
Constructs a RigidTransform from an Eigen Isometry3.
 Parameter
pose
:  Isometry3 that contains an allegedly valid rotation matrix
R_AB
and also contains a position vectorp_AoBo_A
from frame A’s origin to frame B’s origin.p_AoBo_A
must be expressed in frame A.
Raises:  RuntimeError in debug builds if R_AB is not a proper orthonormal
 3x3 rotation matrix.
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix(). __init__(self: pydrake.math.RigidTransform_[AutoDiffXd], pose: numpy.ndarray[object[m, n]]) > None
Constructs a RigidTransform from an appropriate Eigen expression.
 Parameter
pose
:  Generic Eigen matrix expression.
Raises:  RuntimeError if the Eigen expression in pose does not resolve
 to a Vector3 or 3x4 matrix or 4x4 matrix or if the rotational part
 of
pose
is not a proper orthonormal 3x3 rotation matrix or if pose
is a 4x4 matrix whose final row is not[0, 0, 0, 1]
.
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix().Note
This constructor prevents ambiguity that would otherwise exist for a RigidTransform constructor whose argument is an Eigen expression.
const Vector3<double> position(4, 5, 6); const RigidTransform<double> X1(3 * position);  const RotationMatrix<double> R(RollPitchYaw<double>(1, 2, 3)); Eigen::Matrix<double, 3, 4> pose34; pose34 << R.matrix(), position; const RigidTransform<double> X2(1.0 * pose34);  Eigen::Matrix<double, 4, 4> pose4; pose4 << R.matrix(), position, 0, 0, 0, 1; const RigidTransform<double> X3(pose4 * pose4);

template
cast
¶ Instantiations:
cast[AutoDiffXd]

cast[AutoDiffXd]
(self: pydrake.math.RigidTransform_[AutoDiffXd]) → pydrake.math.RigidTransform_[AutoDiffXd]¶ Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T. For example,
RigidTransform<double> source = RigidTransform<double>::Identity(); RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RigidTransform is templated.
Note
RigidTransform<From>::cast<To>()
creates a newRigidTransform<To>
from aRigidTransform<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter


cast[AutoDiffXd]
(self: pydrake.math.RigidTransform_[AutoDiffXd]) → pydrake.math.RigidTransform_[AutoDiffXd]¶ Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T. For example,
RigidTransform<double> source = RigidTransform<double>::Identity(); RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RigidTransform is templated.
Note
RigidTransform<From>::cast<To>()
creates a newRigidTransform<To>
from aRigidTransform<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter

GetAsIsometry3
(self: pydrake.math.RigidTransform_[AutoDiffXd]) → pydrake.common.eigen_geometry.Isometry3_[AutoDiffXd]¶ Returns the isometry in ℜ³ that is equivalent to a RigidTransform.

GetAsMatrix34
(self: pydrake.math.RigidTransform_[AutoDiffXd]) → numpy.ndarray[object[3, 4]]¶ Returns the 3x4 matrix associated with this RigidTransform, i.e., X_AB.
┌ ┐ │ R_AB p_AoBo_A │ └ ┘

GetAsMatrix4
(self: pydrake.math.RigidTransform_[AutoDiffXd]) → numpy.ndarray[object[4, 4]]¶ Returns the 4x4 matrix associated with this RigidTransform, i.e., X_AB.
┌ ┐ │ R_AB p_AoBo_A │ │ │ │ 0 1 │ └ ┘

static
Identity
() → pydrake.math.RigidTransform_[AutoDiffXd]¶ Returns the identity RigidTransform (corresponds to coincident frames).
Returns: the RigidTransform that corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence, the returned RigidTransform contains a 3x3 identity matrix and a zero position vector.

inverse
(self: pydrake.math.RigidTransform_[AutoDiffXd]) → pydrake.math.RigidTransform_[AutoDiffXd]¶ Returns X_BA = X_AB⁻¹, the inverse of
this
RigidTransform.Note
The inverse of RigidTransform X_AB is X_BA, which contains the rotation matrix R_BA = R_AB⁻¹ = R_ABᵀ and the position vector
p_BoAo_B_
(position from B’s origin Bo to A’s origin Ao, expressed in frame B).Note
: The squareroot of a RigidTransform’s condition number is roughly the magnitude of the position vector. The accuracy of the calculation for the inverse of a RigidTransform drops off with the sqrt condition number.

linear
(self: pydrake.math.RigidTransform_[AutoDiffXd]) → numpy.ndarray[object[3, 3]]¶ DO NOT USE! We offer this API for backwards compatibility with Isometry3, but it will be removed on or around 20200701. See drake issue #9865 for details.

matrix
(self: pydrake.math.RigidTransform_[AutoDiffXd]) → numpy.ndarray[object[4, 4]]¶ DO NOT USE! We offer this API for backwards compatibility with Isometry3, but it will be removed on or around 20200701. See drake issue #9865 for details.

multiply
(*args, **kwargs)¶ Overloaded function.
 multiply(self: pydrake.math.RigidTransform_[AutoDiffXd], other: pydrake.math.RigidTransform_[AutoDiffXd]) > pydrake.math.RigidTransform_[AutoDiffXd]
Multiplies
this
RigidTransformX_AB
by theother
RigidTransformX_BC
and returns the RigidTransformX_AC = X_AB * X_BC
. multiply(self: pydrake.math.RigidTransform_[AutoDiffXd], p_BoQ_B: numpy.ndarray[object[3, 1]]) > numpy.ndarray[object[3, 1]]
Multiplies
this
RigidTransformX_AB
by the position vectorp_BoQ_B
which is from Bo (B’s origin) to an arbitrary point Q. Parameter
p_BoQ_B
:  position vector from Bo to Q, expressed in frame B.
 Returns
p_AoQ_A
:  position vector from Ao to Q, expressed in frame A.
 multiply(self: pydrake.math.RigidTransform_[AutoDiffXd], p_BoQ_B: numpy.ndarray[object[3, n]]) > numpy.ndarray[object[3, n]]
Multiplies
this
RigidTransformX_AB
by the n position vectorsp_BoQ1_B
… p_BoQn_B, wherep_BoQi_B
is the iᵗʰ position vector from Bo (frame B’s origin) to an arbitrary point Qi, expressed in frame B. Parameter
p_BoQ_B
: 3 x n
matrix with n position vectorsp_BoQi_B
or an expression that resolves to a3 x n
matrix of position vectors. Returns
p_AoQ_A
: 3 x n
matrix with n position vectorsp_AoQi_A
, i.e., n position vectors from Ao (frame A’s origin) to Qi, expressed in frame A. Specifically, this operator* is defined so thatX_AB * p_BoQ_B
returnsp_AoQ_A = p_AoBo_A + R_AB * p_BoQ_B
, wherep_AoBo_A
is the position vector from Ao to Bo expressed in A andR_AB
is the rotation matrix relating the orientation of frames A and B.
Note
As needed, use parentheses. This operator* is not associative. To see this, let
p = p_AoBo_A
, q = p_BoQ_B and note (X_AB * q) * 7 = (p + R_AB * q) * 7 ≠ X_AB * (q * 7) = p + R_AB * (q * 7).const RollPitchYaw<double> rpy(0.1, 0.2, 0.3); const RigidTransform<double> X_AB(rpy, Vector3d(1, 2, 3)); Eigen::Matrix<double, 3, 2> p_BoQ_B; p_BoQ_B.col(0) = Vector3d(4, 5, 6); p_BoQ_B.col(1) = Vector3d(9, 8, 7); const Eigen::Matrix<double, 3, 2> p_AoQ_A = X_AB * p_BoQ_B;

rotation
(self: pydrake.math.RigidTransform_[AutoDiffXd]) → drake::math::RotationMatrix<Eigen::AutoDiffScalar<Eigen::Matrix<double, 1, 1, 0, 1, 1> > >¶ Returns R_AB, the rotation matrix portion of
this
RigidTransform. Returns
R_AB
:  the rotation matrix portion of
this
RigidTransform.
 Returns

set
(self: pydrake.math.RigidTransform_[AutoDiffXd], R: drake::math::RotationMatrix<Eigen::AutoDiffScalar<Eigen::Matrix<double, 1, 1, 0, 1, 1> > >, p: numpy.ndarray[object[3, 1]]) → None¶ Sets
this
RigidTransform from a RotationMatrix and a position vector. Parameter
R
:  rotation matrix relating frames A and B (e.g.,
R_AB
).  Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
 Parameter

set_rotation
(*args, **kwargs)¶ Overloaded function.
 set_rotation(self: pydrake.math.RigidTransform_[AutoDiffXd], R: drake::math::RotationMatrix<Eigen::AutoDiffScalar<Eigen::Matrix<double, 1, 1, 0, 1, 1> > >) > None
Sets the RotationMatrix portion of
this
RigidTransform. Parameter
R
:  rotation matrix relating frames A and B (e.g.,
R_AB
).
 set_rotation(self: pydrake.math.RigidTransform_[AutoDiffXd], rpy: drake::math::RollPitchYaw<Eigen::AutoDiffScalar<Eigen::Matrix<double, 1, 1, 0, 1, 1> > >) > None
Sets the rotation part of
this
RigidTransform from a RollPitchYaw. Parameter
rpy
:  “rollpitchyaw” angles.
See also
RotationMatrix::RotationMatrix(const RollPitchYaw<T>&) which describes the parameter, preconditions, etc.
 set_rotation(self: pydrake.math.RigidTransform_[AutoDiffXd], quaternion: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd]) > None
Sets the rotation part of
this
RigidTransform from a Quaternion. Parameter
quaternion
:  a quaternion which may or may not have unit length.
See also
RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&) which describes the parameter, preconditions, exception conditions, etc.
 set_rotation(self: pydrake.math.RigidTransform_[AutoDiffXd], theta_lambda: pydrake.common.eigen_geometry.AngleAxis_[AutoDiffXd]) > None
Sets the rotation part of
this
RigidTransform from an AngleAxis. Parameter
theta_lambda
:  an angle
theta
(in radians) and vectorlambda
.
See also
RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&) which describes the parameter, preconditions, exception conditions, etc.

set_translation
(self: pydrake.math.RigidTransform_[AutoDiffXd], p: numpy.ndarray[object[3, 1]]) → None¶ Sets the position vector portion of
this
RigidTransform. Parameter
p
:  position vector from Ao (frame A’s origin) to Bo (frame B’s origin) expressed in frame A. In monogram notation p is denoted p_AoBo_A.
 Parameter

SetFromIsometry3
(self: pydrake.math.RigidTransform_[AutoDiffXd], pose: pydrake.common.eigen_geometry.Isometry3_[AutoDiffXd]) → None¶ Sets
this
RigidTransform from an Eigen Isometry3. Parameter
pose
:  Isometry3 that contains an allegedly valid rotation matrix
R_AB
and also contains a position vectorp_AoBo_A
from frame A’s origin to frame B’s origin.p_AoBo_A
must be expressed in frame A.
Raises:  RuntimeError in debug builds if R_AB is not a proper orthonormal
 3x3 rotation matrix.
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix(). Parameter

SetIdentity
(self: pydrake.math.RigidTransform_[AutoDiffXd]) → pydrake.math.RigidTransform_[AutoDiffXd]¶ Sets
this
RigidTransform so it corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence,this
RigidTransform contains a 3x3 identity matrix and a zero position vector.

translation
(self: pydrake.math.RigidTransform_[AutoDiffXd]) → numpy.ndarray[object[3, 1]]¶ Returns
p_AoBo_A
, the position vector portion ofthis
RigidTransform, i.e., position vector from Ao (frame A’s origin) to Bo (frame B’s origin).

class
pydrake.math.
RigidTransform_[Expression]
¶ This class represents a proper rigid transform between two frames which can be regarded in two ways. A rigid transform describes the “pose” between two frames A and B (i.e., the relative orientation and position of A to B). Alternately, it can be regarded as a distancepreserving operator that can rotate and/or translate a rigid body without changing its shape or size (rigid) and without mirroring/reflecting the body (proper), e.g., it can add one position vector to another and express the result in a particular basis as
p_AoQ_A = X_AB * p_BoQ_B
(Q is any point). In many ways, this rigid transform class is conceptually similar to using a homogeneous matrix as a linear operator. See operator* documentation for an exception.The class stores a RotationMatrix that relates righthanded orthogonal unit vectors Ax, Ay, Az fixed in frame A to righthanded orthogonal unit vectors Bx, By, Bz fixed in frame B. The class also stores a position vector from Ao (the origin of frame A) to Bo (the origin of frame B). The position vector is expressed in frame A. The monogram notation for the transform relating frame A to B is
X_AB
. The monogram notation for the rotation matrix relating A to B isR_AB
. The monogram notation for the position vector from Ao to Bo isp_AoBo_A
. See multibody_quantities for monogram notation for dynamics.Note
This class does not store the frames associated with the transform and cannot enforce correct usage of this class. For example, it makes sense to multiply RigidTransforms as
X_AB * X_BC
, but notX_AB * X_CB
.Note
This class is not a 4x4 transformation matrix – even though its operator*() methods act mostly like 4x4 matrix multiplication. Instead, this class contains a 3x3 rotation matrix class and a 3x1 position vector. To convert this to a 3x4 matrix, use GetAsMatrix34(). To convert this to a 4x4 matrix, use GetAsMatrix4(). To convert this to an Eigen::Isometry, use GetAsIsometry().
Note
An isometry is sometimes regarded as synonymous with rigid transform. The RigidTransform class has important advantages over Eigen::Isometry.  RigidTransform is built on an underlying rigorous 3x3 RotationMatrix class that has significant functionality for 3D orientation.  In Debug builds, RigidTransform requires a valid 3x3 rotation matrix and a valid (nonNAN) position vector. Eigen::Isometry does not.  RigidTransform catches bugs that are undetected by Eigen::Isometry.  RigidTransform has additional functionality and easeofuse, resulting in shorter, easier to write, and easier to read code.  The name Isometry is unfamiliar to many roboticists and dynamicists and for them Isometry.linear() is (for example) a counterintuitive method name to return a rotation matrix.
Note
One of the constructors in this class provides an implicit conversion from an Eigen Translation to RigidTransform.
 Authors:
 Paul Mitiguy (2018) Original author.
 Authors:
 Drake team (see https://drake.mit.edu/credits).

__init__
(*args, **kwargs)¶ Overloaded function.
 __init__(self: pydrake.math.RigidTransform_[Expression]) > None
Constructs the RigidTransform that corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence, the constructed RigidTransform contains an identity RotationMatrix and a zero position vector.
 __init__(self: pydrake.math.RigidTransform_[Expression], other: pydrake.math.RigidTransform_[Expression]) > None
 __init__(self: pydrake.math.RigidTransform_[Expression], R: drake::math::RotationMatrix<drake::symbolic::Expression>, p: numpy.ndarray[object[3, 1]]) > None
Constructs a RigidTransform from a rotation matrix and a position vector.
 Parameter
R
:  rotation matrix relating frames A and B (e.g.,
R_AB
).  Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
 __init__(self: pydrake.math.RigidTransform_[Expression], rpy: drake::math::RollPitchYaw<drake::symbolic::Expression>, p: numpy.ndarray[object[3, 1]]) > None
Constructs a RigidTransform from a RollPitchYaw and a position vector.
 Parameter
rpy
:  a RollPitchYaw which is a Spacefixed (extrinsic) XYZ rotation
with “rollpitchyaw” angles
[r, p, y]
or equivalently a Body fixed (intrinsic) ZYX rotation with “yawpitchroll” angles[y, p, r]
.
See also
RotationMatrix::RotationMatrix(const RollPitchYaw<T>&)
 Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
 __init__(self: pydrake.math.RigidTransform_[Expression], quaternion: pydrake.common.eigen_geometry.Quaternion_[Expression], p: numpy.ndarray[object[3, 1]]) > None
Constructs a RigidTransform from a Quaternion and a position vector.
 Parameter
quaternion
:  a nonzero, finite quaternion which may or may not have unit
length [i.e.,
quaternion.norm()
does not have to be 1].  Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
Raises:  RuntimeError in debug builds if the rotation matrix that is built
 from
quaternion
is invalid.
See also
RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&)
 __init__(self: pydrake.math.RigidTransform_[Expression], theta_lambda: pydrake.common.eigen_geometry.AngleAxis_[Expression], p: numpy.ndarray[object[3, 1]]) > None
Constructs a RigidTransform from an AngleAxis and a position vector.
 Parameter
theta_lambda
:  an Eigen::AngleAxis whose associated axis (vector direction herein
called
lambda
) is nonzero and finite, but which may or may not have unit length [i.e.,lambda.norm()
does not have to be 1].  Parameter
p
:  position vector from frame A’s origin to frame B’s origin, expressed in frame A. In monogram notation p is denoted ``p_AoBo_A
Raises:  RuntimeError in debug builds if the rotation matrix that is built
 from theta_lambda` is invalid.
See also
RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&)
 __init__(self: pydrake.math.RigidTransform_[Expression], R: drake::math::RotationMatrix<drake::symbolic::Expression>) > None
Constructs a RigidTransform with a given RotationMatrix and a zero position vector.
 Parameter
R
:  rotation matrix relating frames A and B (e.g.,
R_AB
).
 __init__(self: pydrake.math.RigidTransform_[Expression], p: numpy.ndarray[object[3, 1]]) > None
Constructs a RigidTransform that contains an identity RotationMatrix and a given position vector
p
. Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
 __init__(self: pydrake.math.RigidTransform_[Expression], pose: pydrake.common.eigen_geometry.Isometry3_[Expression]) > None
Constructs a RigidTransform from an Eigen Isometry3.
 Parameter
pose
:  Isometry3 that contains an allegedly valid rotation matrix
R_AB
and also contains a position vectorp_AoBo_A
from frame A’s origin to frame B’s origin.p_AoBo_A
must be expressed in frame A.
Raises:  RuntimeError in debug builds if R_AB is not a proper orthonormal
 3x3 rotation matrix.
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix(). __init__(self: pydrake.math.RigidTransform_[Expression], pose: numpy.ndarray[object[m, n]]) > None
Constructs a RigidTransform from an appropriate Eigen expression.
 Parameter
pose
:  Generic Eigen matrix expression.
Raises:  RuntimeError if the Eigen expression in pose does not resolve
 to a Vector3 or 3x4 matrix or 4x4 matrix or if the rotational part
 of
pose
is not a proper orthonormal 3x3 rotation matrix or if pose
is a 4x4 matrix whose final row is not[0, 0, 0, 1]
.
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix().Note
This constructor prevents ambiguity that would otherwise exist for a RigidTransform constructor whose argument is an Eigen expression.
const Vector3<double> position(4, 5, 6); const RigidTransform<double> X1(3 * position);  const RotationMatrix<double> R(RollPitchYaw<double>(1, 2, 3)); Eigen::Matrix<double, 3, 4> pose34; pose34 << R.matrix(), position; const RigidTransform<double> X2(1.0 * pose34);  Eigen::Matrix<double, 4, 4> pose4; pose4 << R.matrix(), position, 0, 0, 0, 1; const RigidTransform<double> X3(pose4 * pose4);

template
cast
¶ Instantiations:
cast[Expression]

cast[Expression]
(self: pydrake.math.RigidTransform_[Expression]) → pydrake.math.RigidTransform_[Expression]¶ Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T. For example,
RigidTransform<double> source = RigidTransform<double>::Identity(); RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RigidTransform is templated.
Note
RigidTransform<From>::cast<To>()
creates a newRigidTransform<To>
from aRigidTransform<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter


cast[Expression]
(self: pydrake.math.RigidTransform_[Expression]) → pydrake.math.RigidTransform_[Expression]¶ Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T. For example,
RigidTransform<double> source = RigidTransform<double>::Identity(); RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RigidTransform is templated.
Note
RigidTransform<From>::cast<To>()
creates a newRigidTransform<To>
from aRigidTransform<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter

GetAsIsometry3
(self: pydrake.math.RigidTransform_[Expression]) → pydrake.common.eigen_geometry.Isometry3_[Expression]¶ Returns the isometry in ℜ³ that is equivalent to a RigidTransform.

GetAsMatrix34
(self: pydrake.math.RigidTransform_[Expression]) → numpy.ndarray[object[3, 4]]¶ Returns the 3x4 matrix associated with this RigidTransform, i.e., X_AB.
┌ ┐ │ R_AB p_AoBo_A │ └ ┘

GetAsMatrix4
(self: pydrake.math.RigidTransform_[Expression]) → numpy.ndarray[object[4, 4]]¶ Returns the 4x4 matrix associated with this RigidTransform, i.e., X_AB.
┌ ┐ │ R_AB p_AoBo_A │ │ │ │ 0 1 │ └ ┘

static
Identity
() → pydrake.math.RigidTransform_[Expression]¶ Returns the identity RigidTransform (corresponds to coincident frames).
Returns: the RigidTransform that corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence, the returned RigidTransform contains a 3x3 identity matrix and a zero position vector.

inverse
(self: pydrake.math.RigidTransform_[Expression]) → pydrake.math.RigidTransform_[Expression]¶ Returns X_BA = X_AB⁻¹, the inverse of
this
RigidTransform.Note
The inverse of RigidTransform X_AB is X_BA, which contains the rotation matrix R_BA = R_AB⁻¹ = R_ABᵀ and the position vector
p_BoAo_B_
(position from B’s origin Bo to A’s origin Ao, expressed in frame B).Note
: The squareroot of a RigidTransform’s condition number is roughly the magnitude of the position vector. The accuracy of the calculation for the inverse of a RigidTransform drops off with the sqrt condition number.

linear
(self: pydrake.math.RigidTransform_[Expression]) → numpy.ndarray[object[3, 3]]¶ DO NOT USE! We offer this API for backwards compatibility with Isometry3, but it will be removed on or around 20200701. See drake issue #9865 for details.

matrix
(self: pydrake.math.RigidTransform_[Expression]) → numpy.ndarray[object[4, 4]]¶ DO NOT USE! We offer this API for backwards compatibility with Isometry3, but it will be removed on or around 20200701. See drake issue #9865 for details.

multiply
(*args, **kwargs)¶ Overloaded function.
 multiply(self: pydrake.math.RigidTransform_[Expression], other: pydrake.math.RigidTransform_[Expression]) > pydrake.math.RigidTransform_[Expression]
Multiplies
this
RigidTransformX_AB
by theother
RigidTransformX_BC
and returns the RigidTransformX_AC = X_AB * X_BC
. multiply(self: pydrake.math.RigidTransform_[Expression], p_BoQ_B: numpy.ndarray[object[3, 1]]) > numpy.ndarray[object[3, 1]]
Multiplies
this
RigidTransformX_AB
by the position vectorp_BoQ_B
which is from Bo (B’s origin) to an arbitrary point Q. Parameter
p_BoQ_B
:  position vector from Bo to Q, expressed in frame B.
 Returns
p_AoQ_A
:  position vector from Ao to Q, expressed in frame A.
 multiply(self: pydrake.math.RigidTransform_[Expression], p_BoQ_B: numpy.ndarray[object[3, n]]) > numpy.ndarray[object[3, n]]
Multiplies
this
RigidTransformX_AB
by the n position vectorsp_BoQ1_B
… p_BoQn_B, wherep_BoQi_B
is the iᵗʰ position vector from Bo (frame B’s origin) to an arbitrary point Qi, expressed in frame B. Parameter
p_BoQ_B
: 3 x n
matrix with n position vectorsp_BoQi_B
or an expression that resolves to a3 x n
matrix of position vectors. Returns
p_AoQ_A
: 3 x n
matrix with n position vectorsp_AoQi_A
, i.e., n position vectors from Ao (frame A’s origin) to Qi, expressed in frame A. Specifically, this operator* is defined so thatX_AB * p_BoQ_B
returnsp_AoQ_A = p_AoBo_A + R_AB * p_BoQ_B
, wherep_AoBo_A
is the position vector from Ao to Bo expressed in A andR_AB
is the rotation matrix relating the orientation of frames A and B.
Note
As needed, use parentheses. This operator* is not associative. To see this, let
p = p_AoBo_A
, q = p_BoQ_B and note (X_AB * q) * 7 = (p + R_AB * q) * 7 ≠ X_AB * (q * 7) = p + R_AB * (q * 7).const RollPitchYaw<double> rpy(0.1, 0.2, 0.3); const RigidTransform<double> X_AB(rpy, Vector3d(1, 2, 3)); Eigen::Matrix<double, 3, 2> p_BoQ_B; p_BoQ_B.col(0) = Vector3d(4, 5, 6); p_BoQ_B.col(1) = Vector3d(9, 8, 7); const Eigen::Matrix<double, 3, 2> p_AoQ_A = X_AB * p_BoQ_B;

rotation
(self: pydrake.math.RigidTransform_[Expression]) → drake::math::RotationMatrix<drake::symbolic::Expression>¶ Returns R_AB, the rotation matrix portion of
this
RigidTransform. Returns
R_AB
:  the rotation matrix portion of
this
RigidTransform.
 Returns

set
(self: pydrake.math.RigidTransform_[Expression], R: drake::math::RotationMatrix<drake::symbolic::Expression>, p: numpy.ndarray[object[3, 1]]) → None¶ Sets
this
RigidTransform from a RotationMatrix and a position vector. Parameter
R
:  rotation matrix relating frames A and B (e.g.,
R_AB
).  Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
 Parameter

set_rotation
(*args, **kwargs)¶ Overloaded function.
 set_rotation(self: pydrake.math.RigidTransform_[Expression], R: drake::math::RotationMatrix<drake::symbolic::Expression>) > None
Sets the RotationMatrix portion of
this
RigidTransform. Parameter
R
:  rotation matrix relating frames A and B (e.g.,
R_AB
).
 set_rotation(self: pydrake.math.RigidTransform_[Expression], rpy: drake::math::RollPitchYaw<drake::symbolic::Expression>) > None
Sets the rotation part of
this
RigidTransform from a RollPitchYaw. Parameter
rpy
:  “rollpitchyaw” angles.
See also
RotationMatrix::RotationMatrix(const RollPitchYaw<T>&) which describes the parameter, preconditions, etc.
 set_rotation(self: pydrake.math.RigidTransform_[Expression], quaternion: pydrake.common.eigen_geometry.Quaternion_[Expression]) > None
Sets the rotation part of
this
RigidTransform from a Quaternion. Parameter
quaternion
:  a quaternion which may or may not have unit length.
See also
RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&) which describes the parameter, preconditions, exception conditions, etc.
 set_rotation(self: pydrake.math.RigidTransform_[Expression], theta_lambda: pydrake.common.eigen_geometry.AngleAxis_[Expression]) > None
Sets the rotation part of
this
RigidTransform from an AngleAxis. Parameter
theta_lambda
:  an angle
theta
(in radians) and vectorlambda
.
See also
RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&) which describes the parameter, preconditions, exception conditions, etc.

set_translation
(self: pydrake.math.RigidTransform_[Expression], p: numpy.ndarray[object[3, 1]]) → None¶ Sets the position vector portion of
this
RigidTransform. Parameter
p
:  position vector from Ao (frame A’s origin) to Bo (frame B’s origin) expressed in frame A. In monogram notation p is denoted p_AoBo_A.
 Parameter

SetFromIsometry3
(self: pydrake.math.RigidTransform_[Expression], pose: pydrake.common.eigen_geometry.Isometry3_[Expression]) → None¶ Sets
this
RigidTransform from an Eigen Isometry3. Parameter
pose
:  Isometry3 that contains an allegedly valid rotation matrix
R_AB
and also contains a position vectorp_AoBo_A
from frame A’s origin to frame B’s origin.p_AoBo_A
must be expressed in frame A.
Raises:  RuntimeError in debug builds if R_AB is not a proper orthonormal
 3x3 rotation matrix.
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix(). Parameter

SetIdentity
(self: pydrake.math.RigidTransform_[Expression]) → pydrake.math.RigidTransform_[Expression]¶ Sets
this
RigidTransform so it corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence,this
RigidTransform contains a 3x3 identity matrix and a zero position vector.

translation
(self: pydrake.math.RigidTransform_[Expression]) → numpy.ndarray[object[3, 1]]¶ Returns
p_AoBo_A
, the position vector portion ofthis
RigidTransform, i.e., position vector from Ao (frame A’s origin) to Bo (frame B’s origin).

class
pydrake.math.
RigidTransform_[float]
¶ This class represents a proper rigid transform between two frames which can be regarded in two ways. A rigid transform describes the “pose” between two frames A and B (i.e., the relative orientation and position of A to B). Alternately, it can be regarded as a distancepreserving operator that can rotate and/or translate a rigid body without changing its shape or size (rigid) and without mirroring/reflecting the body (proper), e.g., it can add one position vector to another and express the result in a particular basis as
p_AoQ_A = X_AB * p_BoQ_B
(Q is any point). In many ways, this rigid transform class is conceptually similar to using a homogeneous matrix as a linear operator. See operator* documentation for an exception.The class stores a RotationMatrix that relates righthanded orthogonal unit vectors Ax, Ay, Az fixed in frame A to righthanded orthogonal unit vectors Bx, By, Bz fixed in frame B. The class also stores a position vector from Ao (the origin of frame A) to Bo (the origin of frame B). The position vector is expressed in frame A. The monogram notation for the transform relating frame A to B is
X_AB
. The monogram notation for the rotation matrix relating A to B isR_AB
. The monogram notation for the position vector from Ao to Bo isp_AoBo_A
. See multibody_quantities for monogram notation for dynamics.Note
This class does not store the frames associated with the transform and cannot enforce correct usage of this class. For example, it makes sense to multiply RigidTransforms as
X_AB * X_BC
, but notX_AB * X_CB
.Note
This class is not a 4x4 transformation matrix – even though its operator*() methods act mostly like 4x4 matrix multiplication. Instead, this class contains a 3x3 rotation matrix class and a 3x1 position vector. To convert this to a 3x4 matrix, use GetAsMatrix34(). To convert this to a 4x4 matrix, use GetAsMatrix4(). To convert this to an Eigen::Isometry, use GetAsIsometry().
Note
An isometry is sometimes regarded as synonymous with rigid transform. The RigidTransform class has important advantages over Eigen::Isometry.  RigidTransform is built on an underlying rigorous 3x3 RotationMatrix class that has significant functionality for 3D orientation.  In Debug builds, RigidTransform requires a valid 3x3 rotation matrix and a valid (nonNAN) position vector. Eigen::Isometry does not.  RigidTransform catches bugs that are undetected by Eigen::Isometry.  RigidTransform has additional functionality and easeofuse, resulting in shorter, easier to write, and easier to read code.  The name Isometry is unfamiliar to many roboticists and dynamicists and for them Isometry.linear() is (for example) a counterintuitive method name to return a rotation matrix.
Note
One of the constructors in this class provides an implicit conversion from an Eigen Translation to RigidTransform.
 Authors:
 Paul Mitiguy (2018) Original author.
 Authors:
 Drake team (see https://drake.mit.edu/credits).

__init__
(*args, **kwargs)¶ Overloaded function.
 __init__(self: pydrake.math.RigidTransform_[float]) > None
Constructs the RigidTransform that corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence, the constructed RigidTransform contains an identity RotationMatrix and a zero position vector.
 __init__(self: pydrake.math.RigidTransform_[float], other: pydrake.math.RigidTransform_[float]) > None
 __init__(self: pydrake.math.RigidTransform_[float], R: drake::math::RotationMatrix<double>, p: numpy.ndarray[float64[3, 1]]) > None
Constructs a RigidTransform from a rotation matrix and a position vector.
 Parameter
R
:  rotation matrix relating frames A and B (e.g.,
R_AB
).  Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
 __init__(self: pydrake.math.RigidTransform_[float], rpy: drake::math::RollPitchYaw<double>, p: numpy.ndarray[float64[3, 1]]) > None
Constructs a RigidTransform from a RollPitchYaw and a position vector.
 Parameter
rpy
:  a RollPitchYaw which is a Spacefixed (extrinsic) XYZ rotation
with “rollpitchyaw” angles
[r, p, y]
or equivalently a Body fixed (intrinsic) ZYX rotation with “yawpitchroll” angles[y, p, r]
.
See also
RotationMatrix::RotationMatrix(const RollPitchYaw<T>&)
 Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
 __init__(self: pydrake.math.RigidTransform_[float], quaternion: Eigen::Quaternion<double, 0>, p: numpy.ndarray[float64[3, 1]]) > None
Constructs a RigidTransform from a Quaternion and a position vector.
 Parameter
quaternion
:  a nonzero, finite quaternion which may or may not have unit
length [i.e.,
quaternion.norm()
does not have to be 1].  Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
Raises:  RuntimeError in debug builds if the rotation matrix that is built
 from
quaternion
is invalid.
See also
RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&)
 __init__(self: pydrake.math.RigidTransform_[float], theta_lambda: Eigen::AngleAxis<double>, p: numpy.ndarray[float64[3, 1]]) > None
Constructs a RigidTransform from an AngleAxis and a position vector.
 Parameter
theta_lambda
:  an Eigen::AngleAxis whose associated axis (vector direction herein
called
lambda
) is nonzero and finite, but which may or may not have unit length [i.e.,lambda.norm()
does not have to be 1].  Parameter
p
:  position vector from frame A’s origin to frame B’s origin, expressed in frame A. In monogram notation p is denoted ``p_AoBo_A
Raises:  RuntimeError in debug builds if the rotation matrix that is built
 from theta_lambda` is invalid.
See also
RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&)
 __init__(self: pydrake.math.RigidTransform_[float], R: drake::math::RotationMatrix<double>) > None
Constructs a RigidTransform with a given RotationMatrix and a zero position vector.
 Parameter
R
:  rotation matrix relating frames A and B (e.g.,
R_AB
).
 __init__(self: pydrake.math.RigidTransform_[float], p: numpy.ndarray[float64[3, 1]]) > None
Constructs a RigidTransform that contains an identity RotationMatrix and a given position vector
p
. Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
 __init__(self: pydrake.math.RigidTransform_[float], pose: Eigen::Transform<double, 3, 1, 0>) > None
Constructs a RigidTransform from an Eigen Isometry3.
 Parameter
pose
:  Isometry3 that contains an allegedly valid rotation matrix
R_AB
and also contains a position vectorp_AoBo_A
from frame A’s origin to frame B’s origin.p_AoBo_A
must be expressed in frame A.
Raises:  RuntimeError in debug builds if R_AB is not a proper orthonormal
 3x3 rotation matrix.
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix(). __init__(self: pydrake.math.RigidTransform_[float], pose: numpy.ndarray[float64[m, n]]) > None
Constructs a RigidTransform from an appropriate Eigen expression.
 Parameter
pose
:  Generic Eigen matrix expression.
Raises:  RuntimeError if the Eigen expression in pose does not resolve
 to a Vector3 or 3x4 matrix or 4x4 matrix or if the rotational part
 of
pose
is not a proper orthonormal 3x3 rotation matrix or if pose
is a 4x4 matrix whose final row is not[0, 0, 0, 1]
.
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix().Note
This constructor prevents ambiguity that would otherwise exist for a RigidTransform constructor whose argument is an Eigen expression.
const Vector3<double> position(4, 5, 6); const RigidTransform<double> X1(3 * position);  const RotationMatrix<double> R(RollPitchYaw<double>(1, 2, 3)); Eigen::Matrix<double, 3, 4> pose34; pose34 << R.matrix(), position; const RigidTransform<double> X2(1.0 * pose34);  Eigen::Matrix<double, 4, 4> pose4; pose4 << R.matrix(), position, 0, 0, 0, 1; const RigidTransform<double> X3(pose4 * pose4);

template
cast
¶ Instantiations:
cast[float]
,cast[AutoDiffXd]
,cast[Expression]

cast[float]
(self: pydrake.math.RigidTransform_[float]) → pydrake.math.RigidTransform_[float]¶ Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T. For example,
RigidTransform<double> source = RigidTransform<double>::Identity(); RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RigidTransform is templated.
Note
RigidTransform<From>::cast<To>()
creates a newRigidTransform<To>
from aRigidTransform<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter


cast[AutoDiffXd]
(self: pydrake.math.RigidTransform_[float]) → drake::math::RigidTransform<Eigen::AutoDiffScalar<Eigen::Matrix<double, 1, 1, 0, 1, 1> > >¶ Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T. For example,
RigidTransform<double> source = RigidTransform<double>::Identity(); RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RigidTransform is templated.
Note
RigidTransform<From>::cast<To>()
creates a newRigidTransform<To>
from aRigidTransform<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter

cast[Expression]
(self: pydrake.math.RigidTransform_[float]) → drake::math::RigidTransform<drake::symbolic::Expression>¶ Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T. For example,
RigidTransform<double> source = RigidTransform<double>::Identity(); RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RigidTransform is templated.
Note
RigidTransform<From>::cast<To>()
creates a newRigidTransform<To>
from aRigidTransform<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter

cast[float]
(self: pydrake.math.RigidTransform_[float]) → pydrake.math.RigidTransform_[float]¶ Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T. For example,
RigidTransform<double> source = RigidTransform<double>::Identity(); RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RigidTransform is templated.
Note
RigidTransform<From>::cast<To>()
creates a newRigidTransform<To>
from aRigidTransform<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter

GetAsIsometry3
(self: pydrake.math.RigidTransform_[float]) → Eigen::Transform<double, 3, 1, 0>¶ Returns the isometry in ℜ³ that is equivalent to a RigidTransform.

GetAsMatrix34
(self: pydrake.math.RigidTransform_[float]) → numpy.ndarray[float64[3, 4]]¶ Returns the 3x4 matrix associated with this RigidTransform, i.e., X_AB.
┌ ┐ │ R_AB p_AoBo_A │ └ ┘

GetAsMatrix4
(self: pydrake.math.RigidTransform_[float]) → numpy.ndarray[float64[4, 4]]¶ Returns the 4x4 matrix associated with this RigidTransform, i.e., X_AB.
┌ ┐ │ R_AB p_AoBo_A │ │ │ │ 0 1 │ └ ┘

static
Identity
() → pydrake.math.RigidTransform_[float]¶ Returns the identity RigidTransform (corresponds to coincident frames).
Returns: the RigidTransform that corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence, the returned RigidTransform contains a 3x3 identity matrix and a zero position vector.

inverse
(self: pydrake.math.RigidTransform_[float]) → pydrake.math.RigidTransform_[float]¶ Returns X_BA = X_AB⁻¹, the inverse of
this
RigidTransform.Note
The inverse of RigidTransform X_AB is X_BA, which contains the rotation matrix R_BA = R_AB⁻¹ = R_ABᵀ and the position vector
p_BoAo_B_
(position from B’s origin Bo to A’s origin Ao, expressed in frame B).Note
: The squareroot of a RigidTransform’s condition number is roughly the magnitude of the position vector. The accuracy of the calculation for the inverse of a RigidTransform drops off with the sqrt condition number.

linear
(self: pydrake.math.RigidTransform_[float]) → numpy.ndarray[float64[3, 3]]¶ DO NOT USE! We offer this API for backwards compatibility with Isometry3, but it will be removed on or around 20200701. See drake issue #9865 for details.

matrix
(self: pydrake.math.RigidTransform_[float]) → numpy.ndarray[float64[4, 4]]¶ DO NOT USE! We offer this API for backwards compatibility with Isometry3, but it will be removed on or around 20200701. See drake issue #9865 for details.

multiply
(*args, **kwargs)¶ Overloaded function.
 multiply(self: pydrake.math.RigidTransform_[float], other: pydrake.math.RigidTransform_[float]) > pydrake.math.RigidTransform_[float]
Multiplies
this
RigidTransformX_AB
by theother
RigidTransformX_BC
and returns the RigidTransformX_AC = X_AB * X_BC
. multiply(self: pydrake.math.RigidTransform_[float], p_BoQ_B: numpy.ndarray[float64[3, 1]]) > numpy.ndarray[float64[3, 1]]
Multiplies
this
RigidTransformX_AB
by the position vectorp_BoQ_B
which is from Bo (B’s origin) to an arbitrary point Q. Parameter
p_BoQ_B
:  position vector from Bo to Q, expressed in frame B.
 Returns
p_AoQ_A
:  position vector from Ao to Q, expressed in frame A.
 multiply(self: pydrake.math.RigidTransform_[float], p_BoQ_B: numpy.ndarray[float64[3, n]]) > numpy.ndarray[float64[3, n]]
Multiplies
this
RigidTransformX_AB
by the n position vectorsp_BoQ1_B
… p_BoQn_B, wherep_BoQi_B
is the iᵗʰ position vector from Bo (frame B’s origin) to an arbitrary point Qi, expressed in frame B. Parameter
p_BoQ_B
: 3 x n
matrix with n position vectorsp_BoQi_B
or an expression that resolves to a3 x n
matrix of position vectors. Returns
p_AoQ_A
: 3 x n
matrix with n position vectorsp_AoQi_A
, i.e., n position vectors from Ao (frame A’s origin) to Qi, expressed in frame A. Specifically, this operator* is defined so thatX_AB * p_BoQ_B
returnsp_AoQ_A = p_AoBo_A + R_AB * p_BoQ_B
, wherep_AoBo_A
is the position vector from Ao to Bo expressed in A andR_AB
is the rotation matrix relating the orientation of frames A and B.
Note
As needed, use parentheses. This operator* is not associative. To see this, let
p = p_AoBo_A
, q = p_BoQ_B and note (X_AB * q) * 7 = (p + R_AB * q) * 7 ≠ X_AB * (q * 7) = p + R_AB * (q * 7).const RollPitchYaw<double> rpy(0.1, 0.2, 0.3); const RigidTransform<double> X_AB(rpy, Vector3d(1, 2, 3)); Eigen::Matrix<double, 3, 2> p_BoQ_B; p_BoQ_B.col(0) = Vector3d(4, 5, 6); p_BoQ_B.col(1) = Vector3d(9, 8, 7); const Eigen::Matrix<double, 3, 2> p_AoQ_A = X_AB * p_BoQ_B;

rotation
(self: pydrake.math.RigidTransform_[float]) → drake::math::RotationMatrix<double>¶ Returns R_AB, the rotation matrix portion of
this
RigidTransform. Returns
R_AB
:  the rotation matrix portion of
this
RigidTransform.
 Returns

set
(self: pydrake.math.RigidTransform_[float], R: drake::math::RotationMatrix<double>, p: numpy.ndarray[float64[3, 1]]) → None¶ Sets
this
RigidTransform from a RotationMatrix and a position vector. Parameter
R
:  rotation matrix relating frames A and B (e.g.,
R_AB
).  Parameter
p
:  position vector from frame A’s origin to frame B’s origin,
expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
 Parameter

set_rotation
(*args, **kwargs)¶ Overloaded function.
 set_rotation(self: pydrake.math.RigidTransform_[float], R: drake::math::RotationMatrix<double>) > None
Sets the RotationMatrix portion of
this
RigidTransform. Parameter
R
:  rotation matrix relating frames A and B (e.g.,
R_AB
).
 set_rotation(self: pydrake.math.RigidTransform_[float], rpy: drake::math::RollPitchYaw<double>) > None
Sets the rotation part of
this
RigidTransform from a RollPitchYaw. Parameter
rpy
:  “rollpitchyaw” angles.
See also
RotationMatrix::RotationMatrix(const RollPitchYaw<T>&) which describes the parameter, preconditions, etc.
 set_rotation(self: pydrake.math.RigidTransform_[float], quaternion: Eigen::Quaternion<double, 0>) > None
Sets the rotation part of
this
RigidTransform from a Quaternion. Parameter
quaternion
:  a quaternion which may or may not have unit length.
See also
RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&) which describes the parameter, preconditions, exception conditions, etc.
 set_rotation(self: pydrake.math.RigidTransform_[float], theta_lambda: Eigen::AngleAxis<double>) > None
Sets the rotation part of
this
RigidTransform from an AngleAxis. Parameter
theta_lambda
:  an angle
theta
(in radians) and vectorlambda
.
See also
RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&) which describes the parameter, preconditions, exception conditions, etc.

set_translation
(self: pydrake.math.RigidTransform_[float], p: numpy.ndarray[float64[3, 1]]) → None¶ Sets the position vector portion of
this
RigidTransform. Parameter
p
:  position vector from Ao (frame A’s origin) to Bo (frame B’s origin) expressed in frame A. In monogram notation p is denoted p_AoBo_A.
 Parameter

SetFromIsometry3
(self: pydrake.math.RigidTransform_[float], pose: Eigen::Transform<double, 3, 1, 0>) → None¶ Sets
this
RigidTransform from an Eigen Isometry3. Parameter
pose
:  Isometry3 that contains an allegedly valid rotation matrix
R_AB
and also contains a position vectorp_AoBo_A
from frame A’s origin to frame B’s origin.p_AoBo_A
must be expressed in frame A.
Raises:  RuntimeError in debug builds if R_AB is not a proper orthonormal
 3x3 rotation matrix.
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix(). Parameter

SetIdentity
(self: pydrake.math.RigidTransform_[float]) → pydrake.math.RigidTransform_[float]¶ Sets
this
RigidTransform so it corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence,this
RigidTransform contains a 3x3 identity matrix and a zero position vector.

translation
(self: pydrake.math.RigidTransform_[float]) → numpy.ndarray[float64[3, 1]]¶ Returns
p_AoBo_A
, the position vector portion ofthis
RigidTransform, i.e., position vector from Ao (frame A’s origin) to Bo (frame B’s origin).

pydrake.math.
RollPitchYaw
¶ alias of
pydrake.math.RollPitchYaw_[float]

template
pydrake.math.
RollPitchYaw_
¶ Instantiations:
RollPitchYaw_[float]
,RollPitchYaw_[AutoDiffXd]
,RollPitchYaw_[Expression]

class
RollPitchYaw_[float]
¶ This class represents the orientation between two arbitrary frames A and D associated with a Spacefixed (extrinsic) XYZ rotation by “rollpitchyaw” angles
[r, p, y]
, which is equivalent to a Bodyfixed (intrinsic) ZYX rotation by “yawpitchroll” angles[y, p, r]
. The rotation matrixR_AD
associated with this rollpitchyaw[r, p, y]
rotation sequence is equal to the matrix multiplication shown below.⎡cos(y) sin(y) 0⎤ ⎡ cos(p) 0 sin(p)⎤ ⎡1 0 0 ⎤ R_AD = ⎢sin(y) cos(y) 0⎥ * ⎢ 0 1 0 ⎥ * ⎢0 cos(r) sin(r)⎥ ⎣ 0 0 1⎦ ⎣sin(p) 0 cos(p)⎦ ⎣0 sin(r) cos(r)⎦ = R_AB * R_BC * R_CD
Note
In this discussion, A is the Space frame and D is the Body frame. One way to visualize this rotation sequence is by introducing intermediate frames B and C (useful constructs to understand this rotation sequence). Initially, the frames are aligned so
Di = Ci = Bi = Ai (i = x, y, z)
where Dx, Dy, Dz and Ax, Ay, Az are orthogonal unit vectors fixed in frames D and A respectively. Similarly for Bx, By, Bz and Cx, Cy, Cz in frame B, C. Then D is subjected to successive righthanded rotations relative to A. 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a
roll angle
r
aboutDx = Cx
. Note: D and C are no longer aligned. 2nd rotation R_BC: Frames D, C (collectively – as if welded together)
rotate relative to frame B, A by a pitch angle
p
aboutCy = By
. Note: C and B are no longer aligned. 3rd rotation R_AB: Frames D, C, B (collectively – as if welded)
rotate relative to frame A by a roll angle
y
aboutBz = Az
. Note: B and A are no longer aligned. The monogram notation for the rotation matrix relating A to D isR_AD
.See also
multibody_quantities for monogram notation for dynamics and orientation_discussion “a discussion on rotation matrices”.
Note
This class does not store the frames associated with this rotation sequence.

__init__
(*args, **kwargs)¶ Overloaded function.
 __init__(self: pydrake.math.RollPitchYaw_[float], other: pydrake.math.RollPitchYaw_[float]) > None
 __init__(self: pydrake.math.RollPitchYaw_[float], rpy: numpy.ndarray[float64[3, 1]]) > None
Constructs a RollPitchYaw from a 3x1 array of angles.
 Parameter
rpy
:  3x1 array with roll, pitch, yaw angles (units of radians).
Raises: RuntimeError in debug builds if !IsValid(rpy).  __init__(self: pydrake.math.RollPitchYaw_[float], roll: float, pitch: float, yaw: float) > None
Constructs a RollPitchYaw from roll, pitch, yaw angles (radian units).
 Parameter
roll
:  xdirected angle in SpaceXYZ rotation sequence.
 Parameter
pitch
:  ydirected angle in SpaceXYZ rotation sequence.
 Parameter
yaw
:  zdirected angle in SpaceXYZ rotation sequence.
Raises:  RuntimeError in debug builds if !IsValid(Vector3<T>(roll, pitch,
yaw)).
 __init__(self: pydrake.math.RollPitchYaw_[float], R: pydrake.math.RotationMatrix_[float]) > None
Uses a RotationMatrix to construct a RollPitchYaw with rollpitchyaw angles
[r, p, y]
in the rangeπ <= r <= π
, π/2 <= p <= π/2,π <= y <= π
. Parameter
R
:  a RotationMatrix.
Note
This new highaccuracy algorithm avoids numerical roundoff issues encountered by some algorithms when pitch is within 1E6 of π/2 or π/2.
 __init__(self: pydrake.math.RollPitchYaw_[float], quaternion: Eigen::Quaternion<double, 0>) > None
Uses a Quaternion to construct a RollPitchYaw with rollpitchyaw angles
[r, p, y]
in the rangeπ <= r <= π
, π/2 <= p <= π/2,π <= y <= π
. Parameter
quaternion
:  unit Quaternion.
Note
This new highaccuracy algorithm avoids numerical roundoff issues encountered by some algorithms when pitch is within 1E6 of π/2 or π/2.
Raises: RuntimeError in debug builds if !IsValid(rpy).  __init__(self: pydrake.math.RollPitchYaw_[float], matrix: numpy.ndarray[float64[3, 3]]) > None
Construct from raw rotation matrix. See RotationMatrix overload for more information.

CalcAngularVelocityInChildFromRpyDt
(self: pydrake.math.RollPitchYaw_[float], rpyDt: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 1]]¶ Calculates angular velocity from
this
RollPitchYaw whose rollpitchyaw angles[r; p; y]
relate the orientation of two generic frames A and D. Parameter
rpyDt
:  Timederivative of
[r; p; y]
, i.e.,[ṙ; ṗ; ẏ]
.
Returns: w_AD_D, frame D’s angular velocity in frame A, expressed in D.  Parameter

CalcAngularVelocityInParentFromRpyDt
(self: pydrake.math.RollPitchYaw_[float], rpyDt: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 1]]¶ Calculates angular velocity from
this
RollPitchYaw whose rollpitchyaw angles[r; p; y]
relate the orientation of two generic frames A and D. Parameter
rpyDt
:  Timederivative of
[r; p; y]
, i.e.,[ṙ; ṗ; ẏ]
.
Returns: w_AD_A, frame D’s angular velocity in frame A, expressed in A.  Parameter

CalcRotationMatrixDt
(self: pydrake.math.RollPitchYaw_[float], rpyDt: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 3]]¶ Forms Ṙ, the ordinary derivative of the RotationMatrix
R
with respect to an independent variablet
(t usually denotes time) andR
is the RotationMatrix formed bythis
RollPitchYaw. The rollpitchyaw angles r, p, y are regarded as functions oft
[i.e., r(t), p(t), y(t)]. Parameter
rpyDt
:  Ordinary derivative of rpy with respect to an independent variable
t
(t usually denotes time, but not necessarily).
Returns: Ṙ, the ordinary derivative of R
with respect tot
, calculated as Ṙ = ∂R/∂r * ṙ + ∂R/∂p * ṗ + ∂R/∂y * ẏ. In other words, the returned (i, j) element is ∂Rij/∂r * ṙ + ∂Rij/∂p * ṗ + ∂Rij/∂y * ẏ. Parameter

CalcRpyDDtFromAngularAccelInChild
(self: pydrake.math.RollPitchYaw_[float], rpyDt: numpy.ndarray[float64[3, 1]], alpha_AD_D: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 1]]¶ Uses angular acceleration to compute the 2ⁿᵈ timederivative of
this
RollPitchYaw whose angles[r; p; y]
orient two generic frames A and D. Parameter
rpyDt
:  timederivative of
[r; p; y]
, i.e.,[ṙ; ṗ; ẏ]
.  Parameter
alpha_AD_D
:  , frame D’s angular acceleration in frame A, expressed in frame D.
Returns: [r̈, p̈, ÿ]
, the 2ⁿᵈ timederivative ofthis
RollPitchYaw.Raises: RuntimeError if cos(p) ≈ 0
(p is near gimballock).Note
This method has a dividebyzero error (singularity) when the cosine of the pitch angle
p
is zero [i.e.,cos(p) = 0`]. This problem (called "gimbal lock") occurs when `p = n π + π / 2
, where n is any integer. There are associated precision problems (inaccuracies) in the neighborhood of these pitch angles, i.e., whencos(p) ≈ 0
. Parameter

CalcRpyDDtFromRpyDtAndAngularAccelInParent
(self: pydrake.math.RollPitchYaw_[float], rpyDt: numpy.ndarray[float64[3, 1]], alpha_AD_A: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 1]]¶ Uses angular acceleration to compute the 2ⁿᵈ timederivative of
this
RollPitchYaw whose angles[r; p; y]
orient two generic frames A and D. Parameter
rpyDt
:  timederivative of
[r; p; y]
, i.e.,[ṙ; ṗ; ẏ]
.  Parameter
alpha_AD_A
:  , frame D’s angular acceleration in frame A, expressed in frame A.
Returns: [r̈, p̈, ÿ]
, the 2ⁿᵈ timederivative ofthis
RollPitchYaw.Raises: RuntimeError if cos(p) ≈ 0
(p is near gimballock).Note
This method has a dividebyzero error (singularity) when the cosine of the pitch angle
p
is zero [i.e.,cos(p) = 0`]. This problem (called "gimbal lock") occurs when `p = n π + π / 2
, where n is any integer. There are associated precision problems (inaccuracies) in the neighborhood of these pitch angles, i.e., whencos(p) ≈ 0
. Parameter

CalcRpyDtFromAngularVelocityInParent
(self: pydrake.math.RollPitchYaw_[float], w_AD_A: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 1]]¶ Uses angular velocity to compute the 1ˢᵗ timederivative of
this
RollPitchYaw whose angles[r; p; y]
orient two generic frames A and D. Parameter
w_AD_A
:  , frame D’s angular velocity in frame A, expressed in A.
Returns: [ṙ; ṗ; ẏ]
, the 1ˢᵗ timederivative ofthis
RollPitchYaw.Raises: RuntimeError if cos(p) ≈ 0
(p is near gimballock).Note
This method has a dividebyzero error (singularity) when the cosine of the pitch angle
p
is zero [i.e.,cos(p) = 0`]. This problem (called "gimbal lock") occurs when `p = n π + π / 2
, where n is any integer. There are associated precision problems (inaccuracies) in the neighborhood of these pitch angles, i.e., whencos(p) ≈ 0
. Parameter

pitch_angle
(self: pydrake.math.RollPitchYaw_[float]) → float¶ Returns the pitchangle underlying
this
RollPitchYaw.

roll_angle
(self: pydrake.math.RollPitchYaw_[float]) → float¶ Returns the rollangle underlying
this
RollPitchYaw.

ToQuaternion
(self: pydrake.math.RollPitchYaw_[float]) → Eigen::Quaternion<double, 0>¶ Returns a quaternion representation of
this
RollPitchYaw.

ToRotationMatrix
(self: pydrake.math.RollPitchYaw_[float]) → pydrake.math.RotationMatrix_[float]¶ Returns the RotationMatrix representation of
this
RollPitchYaw.

vector
(self: pydrake.math.RollPitchYaw_[float]) → numpy.ndarray[float64[3, 1]]¶ Returns the Vector3 underlying
this
RollPitchYaw.

yaw_angle
(self: pydrake.math.RollPitchYaw_[float]) → float¶ Returns the yawangle underlying
this
RollPitchYaw.

class

class
pydrake.math.
RollPitchYaw_[AutoDiffXd]
¶ This class represents the orientation between two arbitrary frames A and D associated with a Spacefixed (extrinsic) XYZ rotation by “rollpitchyaw” angles
[r, p, y]
, which is equivalent to a Bodyfixed (intrinsic) ZYX rotation by “yawpitchroll” angles[y, p, r]
. The rotation matrixR_AD
associated with this rollpitchyaw[r, p, y]
rotation sequence is equal to the matrix multiplication shown below.⎡cos(y) sin(y) 0⎤ ⎡ cos(p) 0 sin(p)⎤ ⎡1 0 0 ⎤ R_AD = ⎢sin(y) cos(y) 0⎥ * ⎢ 0 1 0 ⎥ * ⎢0 cos(r) sin(r)⎥ ⎣ 0 0 1⎦ ⎣sin(p) 0 cos(p)⎦ ⎣0 sin(r) cos(r)⎦ = R_AB * R_BC * R_CD
Note
In this discussion, A is the Space frame and D is the Body frame. One way to visualize this rotation sequence is by introducing intermediate frames B and C (useful constructs to understand this rotation sequence). Initially, the frames are aligned so
Di = Ci = Bi = Ai (i = x, y, z)
where Dx, Dy, Dz and Ax, Ay, Az are orthogonal unit vectors fixed in frames D and A respectively. Similarly for Bx, By, Bz and Cx, Cy, Cz in frame B, C. Then D is subjected to successive righthanded rotations relative to A. 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a
roll angle
r
aboutDx = Cx
. Note: D and C are no longer aligned. 2nd rotation R_BC: Frames D, C (collectively – as if welded together)
rotate relative to frame B, A by a pitch angle
p
aboutCy = By
. Note: C and B are no longer aligned. 3rd rotation R_AB: Frames D, C, B (collectively – as if welded)
rotate relative to frame A by a roll angle
y
aboutBz = Az
. Note: B and A are no longer aligned. The monogram notation for the rotation matrix relating A to D isR_AD
.See also
multibody_quantities for monogram notation for dynamics and orientation_discussion “a discussion on rotation matrices”.
Note
This class does not store the frames associated with this rotation sequence.

__init__
(*args, **kwargs)¶ Overloaded function.
 __init__(self: pydrake.math.RollPitchYaw_[AutoDiffXd], other: pydrake.math.RollPitchYaw_[AutoDiffXd]) > None
 __init__(self: pydrake.math.RollPitchYaw_[AutoDiffXd], rpy: numpy.ndarray[object[3, 1]]) > None
Constructs a RollPitchYaw from a 3x1 array of angles.
 Parameter
rpy
:  3x1 array with roll, pitch, yaw angles (units of radians).
Raises: RuntimeError in debug builds if !IsValid(rpy).  __init__(self: pydrake.math.RollPitchYaw_[AutoDiffXd], roll: pydrake.autodiffutils.AutoDiffXd, pitch: pydrake.autodiffutils.AutoDiffXd, yaw: pydrake.autodiffutils.AutoDiffXd) > None
Constructs a RollPitchYaw from roll, pitch, yaw angles (radian units).
 Parameter
roll
:  xdirected angle in SpaceXYZ rotation sequence.
 Parameter
pitch
:  ydirected angle in SpaceXYZ rotation sequence.
 Parameter
yaw
:  zdirected angle in SpaceXYZ rotation sequence.
Raises:  RuntimeError in debug builds if !IsValid(Vector3<T>(roll, pitch,
yaw)).
 __init__(self: pydrake.math.RollPitchYaw_[AutoDiffXd], R: pydrake.math.RotationMatrix_[AutoDiffXd]) > None
Uses a RotationMatrix to construct a RollPitchYaw with rollpitchyaw angles
[r, p, y]
in the rangeπ <= r <= π
, π/2 <= p <= π/2,π <= y <= π
. Parameter
R
:  a RotationMatrix.
Note
This new highaccuracy algorithm avoids numerical roundoff issues encountered by some algorithms when pitch is within 1E6 of π/2 or π/2.
 __init__(self: pydrake.math.RollPitchYaw_[AutoDiffXd], quaternion: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd]) > None
Uses a Quaternion to construct a RollPitchYaw with rollpitchyaw angles
[r, p, y]
in the rangeπ <= r <= π
, π/2 <= p <= π/2,π <= y <= π
. Parameter
quaternion
:  unit Quaternion.
Note
This new highaccuracy algorithm avoids numerical roundoff issues encountered by some algorithms when pitch is within 1E6 of π/2 or π/2.
Raises: RuntimeError in debug builds if !IsValid(rpy).  __init__(self: pydrake.math.RollPitchYaw_[AutoDiffXd], matrix: numpy.ndarray[object[3, 3]]) > None
Construct from raw rotation matrix. See RotationMatrix overload for more information.

CalcAngularVelocityInChildFromRpyDt
(self: pydrake.math.RollPitchYaw_[AutoDiffXd], rpyDt: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 1]]¶ Calculates angular velocity from
this
RollPitchYaw whose rollpitchyaw angles[r; p; y]
relate the orientation of two generic frames A and D. Parameter
rpyDt
:  Timederivative of
[r; p; y]
, i.e.,[ṙ; ṗ; ẏ]
.
Returns: w_AD_D, frame D’s angular velocity in frame A, expressed in D.  Parameter

CalcAngularVelocityInParentFromRpyDt
(self: pydrake.math.RollPitchYaw_[AutoDiffXd], rpyDt: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 1]]¶ Calculates angular velocity from
this
RollPitchYaw whose rollpitchyaw angles[r; p; y]
relate the orientation of two generic frames A and D. Parameter
rpyDt
:  Timederivative of
[r; p; y]
, i.e.,[ṙ; ṗ; ẏ]
.
Returns: w_AD_A, frame D’s angular velocity in frame A, expressed in A.  Parameter

CalcRotationMatrixDt
(self: pydrake.math.RollPitchYaw_[AutoDiffXd], rpyDt: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 3]]¶ Forms Ṙ, the ordinary derivative of the RotationMatrix
R
with respect to an independent variablet
(t usually denotes time) andR
is the RotationMatrix formed bythis
RollPitchYaw. The rollpitchyaw angles r, p, y are regarded as functions oft
[i.e., r(t), p(t), y(t)]. Parameter
rpyDt
:  Ordinary derivative of rpy with respect to an independent variable
t
(t usually denotes time, but not necessarily).
Returns: Ṙ, the ordinary derivative of R
with respect tot
, calculated as Ṙ = ∂R/∂r * ṙ + ∂R/∂p * ṗ + ∂R/∂y * ẏ. In other words, the returned (i, j) element is ∂Rij/∂r * ṙ + ∂Rij/∂p * ṗ + ∂Rij/∂y * ẏ. Parameter

CalcRpyDDtFromAngularAccelInChild
(self: pydrake.math.RollPitchYaw_[AutoDiffXd], rpyDt: numpy.ndarray[object[3, 1]], alpha_AD_D: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 1]]¶ Uses angular acceleration to compute the 2ⁿᵈ timederivative of
this
RollPitchYaw whose angles[r; p; y]
orient two generic frames A and D. Parameter
rpyDt
:  timederivative of
[r; p; y]
, i.e.,[ṙ; ṗ; ẏ]
.  Parameter
alpha_AD_D
:  , frame D’s angular acceleration in frame A, expressed in frame D.
Returns: [r̈, p̈, ÿ]
, the 2ⁿᵈ timederivative ofthis
RollPitchYaw.Raises: RuntimeError if cos(p) ≈ 0
(p is near gimballock).Note
This method has a dividebyzero error (singularity) when the cosine of the pitch angle
p
is zero [i.e.,cos(p) = 0`]. This problem (called "gimbal lock") occurs when `p = n π + π / 2
, where n is any integer. There are associated precision problems (inaccuracies) in the neighborhood of these pitch angles, i.e., whencos(p) ≈ 0
. Parameter

CalcRpyDDtFromRpyDtAndAngularAccelInParent
(self: pydrake.math.RollPitchYaw_[AutoDiffXd], rpyDt: numpy.ndarray[object[3, 1]], alpha_AD_A: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 1]]¶ Uses angular acceleration to compute the 2ⁿᵈ timederivative of
this
RollPitchYaw whose angles[r; p; y]
orient two generic frames A and D. Parameter
rpyDt
:  timederivative of
[r; p; y]
, i.e.,[ṙ; ṗ; ẏ]
.  Parameter
alpha_AD_A
:  , frame D’s angular acceleration in frame A, expressed in frame A.
Returns: [r̈, p̈, ÿ]
, the 2ⁿᵈ timederivative ofthis
RollPitchYaw.Raises: RuntimeError if cos(p) ≈ 0
(p is near gimballock).Note
This method has a dividebyzero error (singularity) when the cosine of the pitch angle
p
is zero [i.e.,cos(p) = 0`]. This problem (called "gimbal lock") occurs when `p = n π + π / 2
, where n is any integer. There are associated precision problems (inaccuracies) in the neighborhood of these pitch angles, i.e., whencos(p) ≈ 0
. Parameter

CalcRpyDtFromAngularVelocityInParent
(self: pydrake.math.RollPitchYaw_[AutoDiffXd], w_AD_A: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 1]]¶ Uses angular velocity to compute the 1ˢᵗ timederivative of
this
RollPitchYaw whose angles[r; p; y]
orient two generic frames A and D. Parameter
w_AD_A
:  , frame D’s angular velocity in frame A, expressed in A.
Returns: [ṙ; ṗ; ẏ]
, the 1ˢᵗ timederivative ofthis
RollPitchYaw.Raises: RuntimeError if cos(p) ≈ 0
(p is near gimballock).Note
This method has a dividebyzero error (singularity) when the cosine of the pitch angle
p
is zero [i.e.,cos(p) = 0`]. This problem (called "gimbal lock") occurs when `p = n π + π / 2
, where n is any integer. There are associated precision problems (inaccuracies) in the neighborhood of these pitch angles, i.e., whencos(p) ≈ 0
. Parameter

pitch_angle
(self: pydrake.math.RollPitchYaw_[AutoDiffXd]) → pydrake.autodiffutils.AutoDiffXd¶ Returns the pitchangle underlying
this
RollPitchYaw.

roll_angle
(self: pydrake.math.RollPitchYaw_[AutoDiffXd]) → pydrake.autodiffutils.AutoDiffXd¶ Returns the rollangle underlying
this
RollPitchYaw.

ToQuaternion
(self: pydrake.math.RollPitchYaw_[AutoDiffXd]) → pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd]¶ Returns a quaternion representation of
this
RollPitchYaw.

ToRotationMatrix
(self: pydrake.math.RollPitchYaw_[AutoDiffXd]) → pydrake.math.RotationMatrix_[AutoDiffXd]¶ Returns the RotationMatrix representation of
this
RollPitchYaw.

vector
(self: pydrake.math.RollPitchYaw_[AutoDiffXd]) → numpy.ndarray[object[3, 1]]¶ Returns the Vector3 underlying
this
RollPitchYaw.

yaw_angle
(self: pydrake.math.RollPitchYaw_[AutoDiffXd]) → pydrake.autodiffutils.AutoDiffXd¶ Returns the yawangle underlying
this
RollPitchYaw.

class
pydrake.math.
RollPitchYaw_[Expression]
¶ This class represents the orientation between two arbitrary frames A and D associated with a Spacefixed (extrinsic) XYZ rotation by “rollpitchyaw” angles
[r, p, y]
, which is equivalent to a Bodyfixed (intrinsic) ZYX rotation by “yawpitchroll” angles[y, p, r]
. The rotation matrixR_AD
associated with this rollpitchyaw[r, p, y]
rotation sequence is equal to the matrix multiplication shown below.⎡cos(y) sin(y) 0⎤ ⎡ cos(p) 0 sin(p)⎤ ⎡1 0 0 ⎤ R_AD = ⎢sin(y) cos(y) 0⎥ * ⎢ 0 1 0 ⎥ * ⎢0 cos(r) sin(r)⎥ ⎣ 0 0 1⎦ ⎣sin(p) 0 cos(p)⎦ ⎣0 sin(r) cos(r)⎦ = R_AB * R_BC * R_CD
Note
In this discussion, A is the Space frame and D is the Body frame. One way to visualize this rotation sequence is by introducing intermediate frames B and C (useful constructs to understand this rotation sequence). Initially, the frames are aligned so
Di = Ci = Bi = Ai (i = x, y, z)
where Dx, Dy, Dz and Ax, Ay, Az are orthogonal unit vectors fixed in frames D and A respectively. Similarly for Bx, By, Bz and Cx, Cy, Cz in frame B, C. Then D is subjected to successive righthanded rotations relative to A. 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a
roll angle
r
aboutDx = Cx
. Note: D and C are no longer aligned. 2nd rotation R_BC: Frames D, C (collectively – as if welded together)
rotate relative to frame B, A by a pitch angle
p
aboutCy = By
. Note: C and B are no longer aligned. 3rd rotation R_AB: Frames D, C, B (collectively – as if welded)
rotate relative to frame A by a roll angle
y
aboutBz = Az
. Note: B and A are no longer aligned. The monogram notation for the rotation matrix relating A to D isR_AD
.See also
multibody_quantities for monogram notation for dynamics and orientation_discussion “a discussion on rotation matrices”.
Note
This class does not store the frames associated with this rotation sequence.

__init__
(*args, **kwargs)¶ Overloaded function.
 __init__(self: pydrake.math.RollPitchYaw_[Expression], other: pydrake.math.RollPitchYaw_[Expression]) > None
 __init__(self: pydrake.math.RollPitchYaw_[Expression], rpy: numpy.ndarray[object[3, 1]]) > None
Constructs a RollPitchYaw from a 3x1 array of angles.
 Parameter
rpy
:  3x1 array with roll, pitch, yaw angles (units of radians).
Raises: RuntimeError in debug builds if !IsValid(rpy).  __init__(self: pydrake.math.RollPitchYaw_[Expression], roll: pydrake.symbolic.Expression, pitch: pydrake.symbolic.Expression, yaw: pydrake.symbolic.Expression) > None
Constructs a RollPitchYaw from roll, pitch, yaw angles (radian units).
 Parameter
roll
:  xdirected angle in SpaceXYZ rotation sequence.
 Parameter
pitch
:  ydirected angle in SpaceXYZ rotation sequence.
 Parameter
yaw
:  zdirected angle in SpaceXYZ rotation sequence.
Raises:  RuntimeError in debug builds if !IsValid(Vector3<T>(roll, pitch,
yaw)).
 __init__(self: pydrake.math.RollPitchYaw_[Expression], R: pydrake.math.RotationMatrix_[Expression]) > None
Uses a RotationMatrix to construct a RollPitchYaw with rollpitchyaw angles
[r, p, y]
in the rangeπ <= r <= π
, π/2 <= p <= π/2,π <= y <= π
. Parameter
R
:  a RotationMatrix.
Note
This new highaccuracy algorithm avoids numerical roundoff issues encountered by some algorithms when pitch is within 1E6 of π/2 or π/2.
 __init__(self: pydrake.math.RollPitchYaw_[Expression], quaternion: pydrake.common.eigen_geometry.Quaternion_[Expression]) > None
Uses a Quaternion to construct a RollPitchYaw with rollpitchyaw angles
[r, p, y]
in the rangeπ <= r <= π
, π/2 <= p <= π/2,π <= y <= π
. Parameter
quaternion
:  unit Quaternion.
Note
This new highaccuracy algorithm avoids numerical roundoff issues encountered by some algorithms when pitch is within 1E6 of π/2 or π/2.
Raises: RuntimeError in debug builds if !IsValid(rpy).  __init__(self: pydrake.math.RollPitchYaw_[Expression], matrix: numpy.ndarray[object[3, 3]]) > None
Construct from raw rotation matrix. See RotationMatrix overload for more information.

CalcAngularVelocityInChildFromRpyDt
(self: pydrake.math.RollPitchYaw_[Expression], rpyDt: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 1]]¶ Calculates angular velocity from
this
RollPitchYaw whose rollpitchyaw angles[r; p; y]
relate the orientation of two generic frames A and D. Parameter
rpyDt
:  Timederivative of
[r; p; y]
, i.e.,[ṙ; ṗ; ẏ]
.
Returns: w_AD_D, frame D’s angular velocity in frame A, expressed in D.  Parameter

CalcAngularVelocityInParentFromRpyDt
(self: pydrake.math.RollPitchYaw_[Expression], rpyDt: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 1]]¶ Calculates angular velocity from
this
RollPitchYaw whose rollpitchyaw angles[r; p; y]
relate the orientation of two generic frames A and D. Parameter
rpyDt
:  Timederivative of
[r; p; y]
, i.e.,[ṙ; ṗ; ẏ]
.
Returns: w_AD_A, frame D’s angular velocity in frame A, expressed in A.  Parameter

CalcRotationMatrixDt
(self: pydrake.math.RollPitchYaw_[Expression], rpyDt: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 3]]¶ Forms Ṙ, the ordinary derivative of the RotationMatrix
R
with respect to an independent variablet
(t usually denotes time) andR
is the RotationMatrix formed bythis
RollPitchYaw. The rollpitchyaw angles r, p, y are regarded as functions oft
[i.e., r(t), p(t), y(t)]. Parameter
rpyDt
:  Ordinary derivative of rpy with respect to an independent variable
t
(t usually denotes time, but not necessarily).
Returns: Ṙ, the ordinary derivative of R
with respect tot
, calculated as Ṙ = ∂R/∂r * ṙ + ∂R/∂p * ṗ + ∂R/∂y * ẏ. In other words, the returned (i, j) element is ∂Rij/∂r * ṙ + ∂Rij/∂p * ṗ + ∂Rij/∂y * ẏ. Parameter

CalcRpyDDtFromAngularAccelInChild
(self: pydrake.math.RollPitchYaw_[Expression], rpyDt: numpy.ndarray[object[3, 1]], alpha_AD_D: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 1]]¶ Uses angular acceleration to compute the 2ⁿᵈ timederivative of
this
RollPitchYaw whose angles[r; p; y]
orient two generic frames A and D. Parameter
rpyDt
:  timederivative of
[r; p; y]
, i.e.,[ṙ; ṗ; ẏ]
.  Parameter
alpha_AD_D
:  , frame D’s angular acceleration in frame A, expressed in frame D.
Returns: [r̈, p̈, ÿ]
, the 2ⁿᵈ timederivative ofthis
RollPitchYaw.Raises: RuntimeError if cos(p) ≈ 0
(p is near gimballock).Note
This method has a dividebyzero error (singularity) when the cosine of the pitch angle
p
is zero [i.e.,cos(p) = 0`]. This problem (called "gimbal lock") occurs when `p = n π + π / 2
, where n is any integer. There are associated precision problems (inaccuracies) in the neighborhood of these pitch angles, i.e., whencos(p) ≈ 0
. Parameter

CalcRpyDDtFromRpyDtAndAngularAccelInParent
(self: pydrake.math.RollPitchYaw_[Expression], rpyDt: numpy.ndarray[object[3, 1]], alpha_AD_A: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 1]]¶ Uses angular acceleration to compute the 2ⁿᵈ timederivative of
this
RollPitchYaw whose angles[r; p; y]
orient two generic frames A and D. Parameter
rpyDt
:  timederivative of
[r; p; y]
, i.e.,[ṙ; ṗ; ẏ]
.  Parameter
alpha_AD_A
:  , frame D’s angular acceleration in frame A, expressed in frame A.
Returns: [r̈, p̈, ÿ]
, the 2ⁿᵈ timederivative ofthis
RollPitchYaw.Raises: RuntimeError if cos(p) ≈ 0
(p is near gimballock).Note
This method has a dividebyzero error (singularity) when the cosine of the pitch angle
p
is zero [i.e.,cos(p) = 0`]. This problem (called "gimbal lock") occurs when `p = n π + π / 2
, where n is any integer. There are associated precision problems (inaccuracies) in the neighborhood of these pitch angles, i.e., whencos(p) ≈ 0
. Parameter

CalcRpyDtFromAngularVelocityInParent
(self: pydrake.math.RollPitchYaw_[Expression], w_AD_A: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 1]]¶ Uses angular velocity to compute the 1ˢᵗ timederivative of
this
RollPitchYaw whose angles[r; p; y]
orient two generic frames A and D. Parameter
w_AD_A
:  , frame D’s angular velocity in frame A, expressed in A.
Returns: [ṙ; ṗ; ẏ]
, the 1ˢᵗ timederivative ofthis
RollPitchYaw.Raises: RuntimeError if cos(p) ≈ 0
(p is near gimballock).Note
This method has a dividebyzero error (singularity) when the cosine of the pitch angle
p
is zero [i.e.,cos(p) = 0`]. This problem (called "gimbal lock") occurs when `p = n π + π / 2
, where n is any integer. There are associated precision problems (inaccuracies) in the neighborhood of these pitch angles, i.e., whencos(p) ≈ 0
. Parameter

pitch_angle
(self: pydrake.math.RollPitchYaw_[Expression]) → pydrake.symbolic.Expression¶ Returns the pitchangle underlying
this
RollPitchYaw.

roll_angle
(self: pydrake.math.RollPitchYaw_[Expression]) → pydrake.symbolic.Expression¶ Returns the rollangle underlying
this
RollPitchYaw.

ToQuaternion
(self: pydrake.math.RollPitchYaw_[Expression]) → pydrake.common.eigen_geometry.Quaternion_[Expression]¶ Returns a quaternion representation of
this
RollPitchYaw.

ToRotationMatrix
(self: pydrake.math.RollPitchYaw_[Expression]) → pydrake.math.RotationMatrix_[Expression]¶ Returns the RotationMatrix representation of
this
RollPitchYaw.

vector
(self: pydrake.math.RollPitchYaw_[Expression]) → numpy.ndarray[object[3, 1]]¶ Returns the Vector3 underlying
this
RollPitchYaw.

yaw_angle
(self: pydrake.math.RollPitchYaw_[Expression]) → pydrake.symbolic.Expression¶ Returns the yawangle underlying
this
RollPitchYaw.

class
pydrake.math.
RollPitchYaw_[float]
¶ This class represents the orientation between two arbitrary frames A and D associated with a Spacefixed (extrinsic) XYZ rotation by “rollpitchyaw” angles
[r, p, y]
, which is equivalent to a Bodyfixed (intrinsic) ZYX rotation by “yawpitchroll” angles[y, p, r]
. The rotation matrixR_AD
associated with this rollpitchyaw[r, p, y]
rotation sequence is equal to the matrix multiplication shown below.⎡cos(y) sin(y) 0⎤ ⎡ cos(p) 0 sin(p)⎤ ⎡1 0 0 ⎤ R_AD = ⎢sin(y) cos(y) 0⎥ * ⎢ 0 1 0 ⎥ * ⎢0 cos(r) sin(r)⎥ ⎣ 0 0 1⎦ ⎣sin(p) 0 cos(p)⎦ ⎣0 sin(r) cos(r)⎦ = R_AB * R_BC * R_CD
Note
In this discussion, A is the Space frame and D is the Body frame. One way to visualize this rotation sequence is by introducing intermediate frames B and C (useful constructs to understand this rotation sequence). Initially, the frames are aligned so
Di = Ci = Bi = Ai (i = x, y, z)
where Dx, Dy, Dz and Ax, Ay, Az are orthogonal unit vectors fixed in frames D and A respectively. Similarly for Bx, By, Bz and Cx, Cy, Cz in frame B, C. Then D is subjected to successive righthanded rotations relative to A. 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a
roll angle
r
aboutDx = Cx
. Note: D and C are no longer aligned. 2nd rotation R_BC: Frames D, C (collectively – as if welded together)
rotate relative to frame B, A by a pitch angle
p
aboutCy = By
. Note: C and B are no longer aligned. 3rd rotation R_AB: Frames D, C, B (collectively – as if welded)
rotate relative to frame A by a roll angle
y
aboutBz = Az
. Note: B and A are no longer aligned. The monogram notation for the rotation matrix relating A to D isR_AD
.See also
multibody_quantities for monogram notation for dynamics and orientation_discussion “a discussion on rotation matrices”.
Note
This class does not store the frames associated with this rotation sequence.

__init__
(*args, **kwargs)¶ Overloaded function.
 __init__(self: pydrake.math.RollPitchYaw_[float], other: pydrake.math.RollPitchYaw_[float]) > None
 __init__(self: pydrake.math.RollPitchYaw_[float], rpy: numpy.ndarray[float64[3, 1]]) > None
Constructs a RollPitchYaw from a 3x1 array of angles.
 Parameter
rpy
:  3x1 array with roll, pitch, yaw angles (units of radians).
Raises: RuntimeError in debug builds if !IsValid(rpy).  __init__(self: pydrake.math.RollPitchYaw_[float], roll: float, pitch: float, yaw: float) > None
Constructs a RollPitchYaw from roll, pitch, yaw angles (radian units).
 Parameter
roll
:  xdirected angle in SpaceXYZ rotation sequence.
 Parameter
pitch
:  ydirected angle in SpaceXYZ rotation sequence.
 Parameter
yaw
:  zdirected angle in SpaceXYZ rotation sequence.
Raises:  RuntimeError in debug builds if !IsValid(Vector3<T>(roll, pitch,
yaw)).
 __init__(self: pydrake.math.RollPitchYaw_[float], R: pydrake.math.RotationMatrix_[float]) > None
Uses a RotationMatrix to construct a RollPitchYaw with rollpitchyaw angles
[r, p, y]
in the rangeπ <= r <= π
, π/2 <= p <= π/2,π <= y <= π
. Parameter
R
:  a RotationMatrix.
Note
This new highaccuracy algorithm avoids numerical roundoff issues encountered by some algorithms when pitch is within 1E6 of π/2 or π/2.
 __init__(self: pydrake.math.RollPitchYaw_[float], quaternion: Eigen::Quaternion<double, 0>) > None
Uses a Quaternion to construct a RollPitchYaw with rollpitchyaw angles
[r, p, y]
in the rangeπ <= r <= π
, π/2 <= p <= π/2,π <= y <= π
. Parameter
quaternion
:  unit Quaternion.
Note
This new highaccuracy algorithm avoids numerical roundoff issues encountered by some algorithms when pitch is within 1E6 of π/2 or π/2.
Raises: RuntimeError in debug builds if !IsValid(rpy).  __init__(self: pydrake.math.RollPitchYaw_[float], matrix: numpy.ndarray[float64[3, 3]]) > None
Construct from raw rotation matrix. See RotationMatrix overload for more information.

CalcAngularVelocityInChildFromRpyDt
(self: pydrake.math.RollPitchYaw_[float], rpyDt: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 1]]¶ Calculates angular velocity from
this
RollPitchYaw whose rollpitchyaw angles[r; p; y]
relate the orientation of two generic frames A and D. Parameter
rpyDt
:  Timederivative of
[r; p; y]
, i.e.,[ṙ; ṗ; ẏ]
.
Returns: w_AD_D, frame D’s angular velocity in frame A, expressed in D.  Parameter

CalcAngularVelocityInParentFromRpyDt
(self: pydrake.math.RollPitchYaw_[float], rpyDt: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 1]]¶ Calculates angular velocity from
this
RollPitchYaw whose rollpitchyaw angles[r; p; y]
relate the orientation of two generic frames A and D. Parameter
rpyDt
:  Timederivative of
[r; p; y]
, i.e.,[ṙ; ṗ; ẏ]
.
Returns: w_AD_A, frame D’s angular velocity in frame A, expressed in A.  Parameter

CalcRotationMatrixDt
(self: pydrake.math.RollPitchYaw_[float], rpyDt: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 3]]¶ Forms Ṙ, the ordinary derivative of the RotationMatrix
R
with respect to an independent variablet
(t usually denotes time) andR
is the RotationMatrix formed bythis
RollPitchYaw. The rollpitchyaw angles r, p, y are regarded as functions oft
[i.e., r(t), p(t), y(t)]. Parameter
rpyDt
:  Ordinary derivative of rpy with respect to an independent variable
t
(t usually denotes time, but not necessarily).
Returns: Ṙ, the ordinary derivative of R
with respect tot
, calculated as Ṙ = ∂R/∂r * ṙ + ∂R/∂p * ṗ + ∂R/∂y * ẏ. In other words, the returned (i, j) element is ∂Rij/∂r * ṙ + ∂Rij/∂p * ṗ + ∂Rij/∂y * ẏ. Parameter

CalcRpyDDtFromAngularAccelInChild
(self: pydrake.math.RollPitchYaw_[float], rpyDt: numpy.ndarray[float64[3, 1]], alpha_AD_D: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 1]]¶ Uses angular acceleration to compute the 2ⁿᵈ timederivative of
this
RollPitchYaw whose angles[r; p; y]
orient two generic frames A and D. Parameter
rpyDt
:  timederivative of
[r; p; y]
, i.e.,[ṙ; ṗ; ẏ]
.  Parameter
alpha_AD_D
:  , frame D’s angular acceleration in frame A, expressed in frame D.
Returns: [r̈, p̈, ÿ]
, the 2ⁿᵈ timederivative ofthis
RollPitchYaw.Raises: RuntimeError if cos(p) ≈ 0
(p is near gimballock).Note
This method has a dividebyzero error (singularity) when the cosine of the pitch angle
p
is zero [i.e.,cos(p) = 0`]. This problem (called "gimbal lock") occurs when `p = n π + π / 2
, where n is any integer. There are associated precision problems (inaccuracies) in the neighborhood of these pitch angles, i.e., whencos(p) ≈ 0
. Parameter

CalcRpyDDtFromRpyDtAndAngularAccelInParent
(self: pydrake.math.RollPitchYaw_[float], rpyDt: numpy.ndarray[float64[3, 1]], alpha_AD_A: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 1]]¶ Uses angular acceleration to compute the 2ⁿᵈ timederivative of
this
RollPitchYaw whose angles[r; p; y]
orient two generic frames A and D. Parameter
rpyDt
:  timederivative of
[r; p; y]
, i.e.,[ṙ; ṗ; ẏ]
.  Parameter
alpha_AD_A
:  , frame D’s angular acceleration in frame A, expressed in frame A.
Returns: [r̈, p̈, ÿ]
, the 2ⁿᵈ timederivative ofthis
RollPitchYaw.Raises: RuntimeError if cos(p) ≈ 0
(p is near gimballock).Note
This method has a dividebyzero error (singularity) when the cosine of the pitch angle
p
is zero [i.e.,cos(p) = 0`]. This problem (called "gimbal lock") occurs when `p = n π + π / 2
, where n is any integer. There are associated precision problems (inaccuracies) in the neighborhood of these pitch angles, i.e., whencos(p) ≈ 0
. Parameter

CalcRpyDtFromAngularVelocityInParent
(self: pydrake.math.RollPitchYaw_[float], w_AD_A: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 1]]¶ Uses angular velocity to compute the 1ˢᵗ timederivative of
this
RollPitchYaw whose angles[r; p; y]
orient two generic frames A and D. Parameter
w_AD_A
:  , frame D’s angular velocity in frame A, expressed in A.
Returns: [ṙ; ṗ; ẏ]
, the 1ˢᵗ timederivative ofthis
RollPitchYaw.Raises: RuntimeError if cos(p) ≈ 0
(p is near gimballock).Note
This method has a dividebyzero error (singularity) when the cosine of the pitch angle
p
is zero [i.e.,cos(p) = 0`]. This problem (called "gimbal lock") occurs when `p = n π + π / 2
, where n is any integer. There are associated precision problems (inaccuracies) in the neighborhood of these pitch angles, i.e., whencos(p) ≈ 0
. Parameter

pitch_angle
(self: pydrake.math.RollPitchYaw_[float]) → float¶ Returns the pitchangle underlying
this
RollPitchYaw.

roll_angle
(self: pydrake.math.RollPitchYaw_[float]) → float¶ Returns the rollangle underlying
this
RollPitchYaw.

ToQuaternion
(self: pydrake.math.RollPitchYaw_[float]) → Eigen::Quaternion<double, 0>¶ Returns a quaternion representation of
this
RollPitchYaw.

ToRotationMatrix
(self: pydrake.math.RollPitchYaw_[float]) → pydrake.math.RotationMatrix_[float]¶ Returns the RotationMatrix representation of
this
RollPitchYaw.

vector
(self: pydrake.math.RollPitchYaw_[float]) → numpy.ndarray[float64[3, 1]]¶ Returns the Vector3 underlying
this
RollPitchYaw.

yaw_angle
(self: pydrake.math.RollPitchYaw_[float]) → float¶ Returns the yawangle underlying
this
RollPitchYaw.

pydrake.math.
RotationMatrix
¶ alias of
pydrake.math.RotationMatrix_[float]

template
pydrake.math.
RotationMatrix_
¶ Instantiations:
RotationMatrix_[float]
,RotationMatrix_[AutoDiffXd]
,RotationMatrix_[Expression]

class
RotationMatrix_[float]
¶ This class represents a 3x3 rotation matrix between two arbitrary frames A and B and helps ensure users create valid rotation matrices. This class relates righthanded orthogonal unit vectors Ax, Ay, Az fixed in frame A to righthanded orthogonal unit vectors Bx, By, Bz fixed in frame B. The monogram notation for the rotation matrix relating A to B is
R_AB
. An example that gives context to this rotation matrix isv_A = R_AB * v_B
, wherev_B
denotes an arbitrary vector v expressed in terms of Bx, By, Bz andv_A
denotes vector v expressed in terms of Ax, Ay, Az. See multibody_quantities for monogram notation for dynamics. See orientation_discussion “a discussion on rotation matrices”.Note
This class does not store the frames associated with a rotation matrix nor does it enforce strict proper usage of this class with vectors.
Note
When assertions are enabled, several methods in this class do a validity check and throw an exception (RuntimeError) if the rotation matrix is invalid. When assertions are disabled, many of these validity checks are skipped (which helps improve speed). In addition, these validity tests are only performed for scalar types for which drake::scalar_predicate<T>::is_bool is
True
. For instance, validity checks are not performed when T is symbolic::Expression. Authors:
 Paul Mitiguy (2018) Original author.
 Authors:
 Drake team (see https://drake.mit.edu/credits).

__init__
(*args, **kwargs)¶ Overloaded function.
 __init__(self: pydrake.math.RotationMatrix_[float]) > None
Constructs a 3x3 identity RotationMatrix – which corresponds to aligning two frames (so that unit vectors Ax = Bx, Ay = By, Az = Bz).
 __init__(self: pydrake.math.RotationMatrix_[float], other: pydrake.math.RotationMatrix_[float]) > None
 __init__(self: pydrake.math.RotationMatrix_[float], R: numpy.ndarray[float64[3, 3]]) > None
Constructs a RotationMatrix from a Matrix3.
 Parameter
R
:  an allegedly valid rotation matrix.
Raises: RuntimeError in debug builds if R fails IsValid(R).  __init__(self: pydrake.math.RotationMatrix_[float], quaternion: Eigen::Quaternion<double, 0>) > None
Constructs a RotationMatrix from an Eigen::Quaternion.
 Parameter
quaternion
:  a nonzero, finite quaternion which may or may not have unit
length [i.e.,
quaternion.norm()
does not have to be 1].
Raises:  RuntimeError in debug builds if the rotation matrix R that is
 built from
quaternion
fails IsValid(R). For example, an  exception is thrown if
quaternion
is zero or contains a NaN or infinity.
Note
This method has the effect of normalizing its
quaternion
argument, without the inefficiency of the squareroot associated with normalization. __init__(self: pydrake.math.RotationMatrix_[float], theta_lambda: Eigen::AngleAxis<double>) > None
Constructs a RotationMatrix from an Eigen::AngleAxis.
 Parameter
theta_lambda
:  an Eigen::AngleAxis whose associated axis (vector direction herein
called
lambda
) is nonzero and finite, but which may or may not have unit length [i.e.,lambda.norm()
does not have to be 1].
Raises:  RuntimeError in debug builds if the rotation matrix R that is
 built from
theta_lambda
fails IsValid(R). For example, an  exception is thrown if
lambda
is zero or contains a NaN or infinity.
 __init__(self: pydrake.math.RotationMatrix_[float], rpy: drake::math::RollPitchYaw<double>) > None
Constructs a RotationMatrix from an RollPitchYaw. In other words, makes the RotationMatrix for a Spacefixed (extrinsic) XYZ rotation by “rollpitchyaw” angles
[r, p, y]
, which is equivalent to a Bodyfixed (intrinsic) ZYX rotation by “yawpitchroll” angles[y, p, r]
. Parameter
rpy
:  radian measures of three angles [roll, pitch, yaw].
 Parameter
rpy
:  a RollPitchYaw which is a Spacefixed (extrinsic) XYZ rotation
with “rollpitchyaw” angles
[r, p, y]
or equivalently a Body fixed (intrinsic) ZYX rotation with “yawpitchroll” angles[y, p, r]
.
Note
Denoting roll
r
, pitchp
, yawy
, this method returns a rotation matrixR_AD
equal to the matrix multiplication shown below.⎡cos(y) sin(y) 0⎤ ⎡ cos(p) 0 sin(p)⎤ ⎡1 0 0 ⎤ R_AD = ⎢sin(y) cos(y) 0⎥ * ⎢ 0 1 0 ⎥ * ⎢0 cos(r) sin(r)⎥ ⎣ 0 0 1⎦ ⎣sin(p) 0 cos(p)⎦ ⎣0 sin(r) cos(r)⎦ = R_AB * R_BC * R_CD
Note
In this discussion, A is the Space frame and D is the Body frame. One way to visualize this rotation sequence is by introducing intermediate frames B and C (useful constructs to understand this rotation sequence). Initially, the frames are aligned so
Di = Ci = Bi = Ai (i = x, y, z)
. Then D is subjected to successive righthanded rotations relative to A. 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a
roll angle
r
aboutDx = Cx
. Note: D and C are no longer aligned. 2nd rotation R_BC: Frames D, C (collectively – as if welded together)
rotate relative to frame B, A by a pitch angle
p
aboutCy = By
. Note: C and B are no longer aligned. 3rd rotation R_AB: Frames D, C, B (collectively – as if welded)
rotate relative to frame A by a roll angle
y
aboutBz = Az
. Note: B and A are no longer aligned.Note
This method constructs a RotationMatrix from a RollPitchYaw. Viceversa, there are highaccuracy RollPitchYaw constructor/methods that form a RollPitchYaw from a rotation matrix.

template
cast
¶ Instantiations:
cast[float]
,cast[AutoDiffXd]
,cast[Expression]

cast[float]
(self: pydrake.math.RotationMatrix_[float]) → pydrake.math.RotationMatrix_[float]¶ Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T. For example,
RotationMatrix<double> source = RotationMatrix<double>::Identity(); RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>()
creates a newRotationMatrix<To>
from aRotationMatrix<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter


cast[AutoDiffXd]
(self: pydrake.math.RotationMatrix_[float]) → drake::math::RotationMatrix<Eigen::AutoDiffScalar<Eigen::Matrix<double, 1, 1, 0, 1, 1> > >¶ Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T. For example,
RotationMatrix<double> source = RotationMatrix<double>::Identity(); RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>()
creates a newRotationMatrix<To>
from aRotationMatrix<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter

cast[Expression]
(self: pydrake.math.RotationMatrix_[float]) → drake::math::RotationMatrix<drake::symbolic::Expression>¶ Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T. For example,
RotationMatrix<double> source = RotationMatrix<double>::Identity(); RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>()
creates a newRotationMatrix<To>
from aRotationMatrix<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter

cast[float]
(self: pydrake.math.RotationMatrix_[float]) → pydrake.math.RotationMatrix_[float]¶ Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T. For example,
RotationMatrix<double> source = RotationMatrix<double>::Identity(); RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>()
creates a newRotationMatrix<To>
from aRotationMatrix<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter

col
(self: pydrake.math.RotationMatrix_[float], index: int) → numpy.ndarray[float64[3, 1]]¶ Returns
this
rotation matrix’s iᵗʰ column (i = 0, 1, 2). Forthis
rotation matrix R_AB (which relates righthanded sets of orthogonal unit vectors Ax, Ay, Az to Bx, By, Bz),  col(0) returns Bx_A (Bx expressed in terms of Ax, Ay, Az).  col(1) returns By_A (By expressed in terms of Ax, Ay, Az).  col(2) returns Bz_A (Bz expressed in terms of Ax, Ay, Az). Parameter
index
:  requested column index (0 <= index <= 2).
See also
row(), matrix()
Raises: In debug builds, asserts (0 <= index <= 2). Note
For efficiency and consistency with Eigen, this method returns the same quantity returned by Eigen’s col() operator. The returned quantity can be assigned in various ways, e.g., as
const auto& Bz_A = col(2);
orVector3<T> Bz_A = col(2);
 Parameter

static
Identity
() → pydrake.math.RotationMatrix_[float]¶

inverse
(self: pydrake.math.RotationMatrix_[float]) → pydrake.math.RotationMatrix_[float]¶

IsExactlyIdentity
(self: pydrake.math.RotationMatrix_[float]) → bool¶ Returns
True
ifthis
is exactly equal to the identity matrix.

IsIdentityToInternalTolerance
(self: pydrake.math.RotationMatrix_[float]) → bool¶ Returns true if
this
is equal to the identity matrix to within the threshold of get_internal_tolerance_for_orthonormality().

IsValid
(self: pydrake.math.RotationMatrix_[float]) → bool¶ Tests if
this
rotation matrix R is a proper orthonormal rotation matrix to within the threshold of get_internal_tolerance_for_orthonormality().Returns: True
ifthis
is a valid rotation matrix.

static
MakeXRotation
(theta: float) → pydrake.math.RotationMatrix_[float]¶ Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAx = Bx
. Parameter
theta
:  radian measure of rotation angle about Ax.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitX().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a righthanded rotation relative to A by an angletheta
aboutAx = Bx
.⎡ 1 0 0 ⎤ R_AB = ⎢ 0 cos(theta) sin(theta) ⎥ ⎣ 0 sin(theta) cos(theta) ⎦
 Parameter

static
MakeYRotation
(theta: float) → pydrake.math.RotationMatrix_[float]¶ Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAy = By
. Parameter
theta
:  radian measure of rotation angle about Ay.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitY().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a righthanded rotation relative to A by an angletheta
aboutAy = By
.⎡ cos(theta) 0 sin(theta) ⎤ R_AB = ⎢ 0 1 0 ⎥ ⎣ sin(theta) 0 cos(theta) ⎦
 Parameter

static
MakeZRotation
(theta: float) → pydrake.math.RotationMatrix_[float]¶ Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAz = Bz
. Parameter
theta
:  radian measure of rotation angle about Az.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitZ().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a righthanded rotation relative to A by an angletheta
aboutAz = Bz
.⎡ cos(theta) sin(theta) 0 ⎤ R_AB = ⎢ sin(theta) cos(theta) 0 ⎥ ⎣ 0 0 1 ⎦
 Parameter

matrix
(self: pydrake.math.RotationMatrix_[float]) → numpy.ndarray[float64[3, 3]]¶ Returns the Matrix3 underlying a RotationMatrix.
See also
col(), row()

multiply
(*args, **kwargs)¶ Overloaded function.
 multiply(self: pydrake.math.RotationMatrix_[float], other: pydrake.math.RotationMatrix_[float]) > pydrake.math.RotationMatrix_[float]
Calculates
this
rotation matrixR_AB
multiplied byother
rotation matrixR_BC
, returning the compositionR_AB * R_BC
. Parameter
other
:  RotationMatrix that postmultiplies
this
.
Returns: rotation matrix that results from this
multiplied byother
.Note
It is possible (albeit improbable) to create an invalid rotation matrix by accumulating roundoff error with a large number of multiplies.
 multiply(self: pydrake.math.RotationMatrix_[float], v_B: numpy.ndarray[float64[3, 1]]) > numpy.ndarray[float64[3, 1]]
Calculates
this
rotation matrixR_AB
multiplied by an arbitrary Vector3 expressed in the B frame. Parameter
v_B
:  3x1 vector that postmultiplies
this
.
Returns: 3x1 vector v_A = R_AB * v_B
. multiply(self: pydrake.math.RotationMatrix_[float], v_B: numpy.ndarray[float64[3, n]]) > numpy.ndarray[float64[3, n]]
Multiplies
this
RotationMatrixR_AB
by the n vectorsv1
, … vn, where each vector has 3 elements and is expressed in frame B. Parameter
v_B
: 3 x n
matrix whose n columns are regarded as arbitrary vectorsv1
, … vn expressed in frame B. Returns
v_A
: 3 x n
matrix whose n columns are vectorsv1
, … vn expressed in frame A.
const RollPitchYaw<double> rpy(0.1, 0.2, 0.3); const RotationMatrix<double> R_AB(rpy); Eigen::Matrix<double, 3, 2> v_B; v_B.col(0) = Vector3d(4, 5, 6); v_B.col(1) = Vector3d(9, 8, 7); const Eigen::Matrix<double, 3, 2> v_A = R_AB * v_B;

static
ProjectToRotationMatrix
(M: numpy.ndarray[float64[3, 3]]) → pydrake.math.RotationMatrix_[float]¶ Given an approximate rotation matrix M, finds the RotationMatrix R closest to M. Closeness is measured with a matrix2 norm (or equivalently with a Frobenius norm). Hence, this method creates a RotationMatrix R from a 3x3 matrix M by minimizing
‖R  M‖₂
(the matrix2 norm of (RM)) subject toR * Rᵀ = I
, where I is the 3x3 identity matrix. For this problem, closeness can also be measured by forming the orthonormal matrix R whose elements minimize the doublesummation∑ᵢ ∑ⱼ (R(i,j)  M(i,j))²
wherei = 1:3, j = 1:3
, subject toR * Rᵀ = I
. The squareroot of this doublesummation is called the Frobenius norm. Parameter
M
:  a 3x3 matrix.
 Parameter
quality_factor
:  . The quality of M as a rotation matrix.
quality_factor
= 1 is perfect (M = R).quality_factor
= 1.25 means that when M multiplies a unit vector (magnitude 1), a vector of magnitude as large as 1.25 may result.quality_factor
= 0.8 means that when M multiplies a unit vector, a vector of magnitude as small as 0.8 may result.quality_factor
= 0 means M is singular, so at least one of the bases related by matrix M does not span 3D space (when M multiples a unit vector, a vector of magnitude as small as 0 may result).
Returns: proper orthonormal matrix R that is closest to M. Raises: RuntimeError if R fails IsValid(R). Note
William Kahan (UC Berkeley) and Hongkai Dai (Toyota Research Institute) proved that for this problem, the same R that minimizes the Frobenius norm also minimizes the matrix2 norm (a.k.a an induced2 norm), which is defined [Dahleh, Section 4.2] as the column matrix u which maximizes
‖(R  M) u‖ / ‖u‖
, whereu ≠ 0
. Since the matrix2 norm of any matrix A is equal to the maximum singular value of A, minimizing the matrix2 norm of (R  M) is equivalent to minimizing the maximum singular value of (R  M). [Dahleh] “Lectures on Dynamic Systems and Controls: Electrical
Engineering and Computer Science, Massachusetts Institute of Technology” https://ocw.mit.edu/courses/electricalengineeringandcomputerscience/6241jdynamicsystemsandcontrolspring2011/readings/MIT6_241JS11_chap04.pdf
 Parameter

row
(self: pydrake.math.RotationMatrix_[float], index: int) → numpy.ndarray[float64[1, 3]]¶ Returns
this
rotation matrix’s iᵗʰ row (i = 0, 1, 2). Forthis
rotation matrix R_AB (which relates righthanded sets of orthogonal unit vectors Ax, Ay, Az to Bx, By, Bz),  row(0) returns Ax_B (Ax expressed in terms of Bx, By, Bz).  row(1) returns Ay_B (Ay expressed in terms of Bx, By, Bz).  row(2) returns Az_B (Az expressed in terms of Bx, By, Bz). Parameter
index
:  requested row index (0 <= index <= 2).
See also
col(), matrix()
Raises: In debug builds, asserts (0 <= index <= 2). Note
For efficiency and consistency with Eigen, this method returns the same quantity returned by Eigen’s row() operator. The returned quantity can be assigned in various ways, e.g., as
const auto& Az_B = row(2);
orRowVector3<T> Az_B = row(2);
 Parameter

set
(self: pydrake.math.RotationMatrix_[float], R: numpy.ndarray[float64[3, 3]]) → None¶ Sets
this
RotationMatrix from a Matrix3. Parameter
R
:  an allegedly valid rotation matrix.
Raises: RuntimeError in debug builds if R fails IsValid(R).  Parameter

ToAngleAxis
(self: pydrake.math.RotationMatrix_[float]) → Eigen::AngleAxis<double>¶ Returns an AngleAxis
theta_lambda
containing an angletheta
and unit vector (axis direction)lambda
that representsthis
RotationMatrix.Note
The orientation and RotationMatrix associated with
theta * lambda
is identical to that of(theta) * (lambda)
. The AngleAxis returned by this method chooses to have0 <= theta <= pi
.Returns: an AngleAxis with 0 <= theta <= pi
and a unit vectorlambda
.

ToQuaternion
(self: pydrake.math.RotationMatrix_[float]) → Eigen::Quaternion<double, 0>¶ Returns a quaternion q that represents
this
RotationMatrix. Since the quaternionq
andq
represent the same RotationMatrix, this method chooses to return a canonical quaternion, i.e., with q(0) >= 0.Note
There is a constructor in the RollPitchYaw class that converts a rotation matrix to rollpitchyaw angles.

transpose
(self: pydrake.math.RotationMatrix_[float]) → pydrake.math.RotationMatrix_[float]¶

class

class
pydrake.math.
RotationMatrix_[AutoDiffXd]
¶ This class represents a 3x3 rotation matrix between two arbitrary frames A and B and helps ensure users create valid rotation matrices. This class relates righthanded orthogonal unit vectors Ax, Ay, Az fixed in frame A to righthanded orthogonal unit vectors Bx, By, Bz fixed in frame B. The monogram notation for the rotation matrix relating A to B is
R_AB
. An example that gives context to this rotation matrix isv_A = R_AB * v_B
, wherev_B
denotes an arbitrary vector v expressed in terms of Bx, By, Bz andv_A
denotes vector v expressed in terms of Ax, Ay, Az. See multibody_quantities for monogram notation for dynamics. See orientation_discussion “a discussion on rotation matrices”.Note
This class does not store the frames associated with a rotation matrix nor does it enforce strict proper usage of this class with vectors.
Note
When assertions are enabled, several methods in this class do a validity check and throw an exception (RuntimeError) if the rotation matrix is invalid. When assertions are disabled, many of these validity checks are skipped (which helps improve speed). In addition, these validity tests are only performed for scalar types for which drake::scalar_predicate<T>::is_bool is
True
. For instance, validity checks are not performed when T is symbolic::Expression. Authors:
 Paul Mitiguy (2018) Original author.
 Authors:
 Drake team (see https://drake.mit.edu/credits).

__init__
(*args, **kwargs)¶ Overloaded function.
 __init__(self: pydrake.math.RotationMatrix_[AutoDiffXd]) > None
Constructs a 3x3 identity RotationMatrix – which corresponds to aligning two frames (so that unit vectors Ax = Bx, Ay = By, Az = Bz).
 __init__(self: pydrake.math.RotationMatrix_[AutoDiffXd], other: pydrake.math.RotationMatrix_[AutoDiffXd]) > None
 __init__(self: pydrake.math.RotationMatrix_[AutoDiffXd], R: numpy.ndarray[object[3, 3]]) > None
Constructs a RotationMatrix from a Matrix3.
 Parameter
R
:  an allegedly valid rotation matrix.
Raises: RuntimeError in debug builds if R fails IsValid(R).  __init__(self: pydrake.math.RotationMatrix_[AutoDiffXd], quaternion: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd]) > None
Constructs a RotationMatrix from an Eigen::Quaternion.
 Parameter
quaternion
:  a nonzero, finite quaternion which may or may not have unit
length [i.e.,
quaternion.norm()
does not have to be 1].
Raises:  RuntimeError in debug builds if the rotation matrix R that is
 built from
quaternion
fails IsValid(R). For example, an  exception is thrown if
quaternion
is zero or contains a NaN or infinity.
Note
This method has the effect of normalizing its
quaternion
argument, without the inefficiency of the squareroot associated with normalization. __init__(self: pydrake.math.RotationMatrix_[AutoDiffXd], theta_lambda: pydrake.common.eigen_geometry.AngleAxis_[AutoDiffXd]) > None
Constructs a RotationMatrix from an Eigen::AngleAxis.
 Parameter
theta_lambda
:  an Eigen::AngleAxis whose associated axis (vector direction herein
called
lambda
) is nonzero and finite, but which may or may not have unit length [i.e.,lambda.norm()
does not have to be 1].
Raises:  RuntimeError in debug builds if the rotation matrix R that is
 built from
theta_lambda
fails IsValid(R). For example, an  exception is thrown if
lambda
is zero or contains a NaN or infinity.
 __init__(self: pydrake.math.RotationMatrix_[AutoDiffXd], rpy: drake::math::RollPitchYaw<Eigen::AutoDiffScalar<Eigen::Matrix<double, 1, 1, 0, 1, 1> > >) > None
Constructs a RotationMatrix from an RollPitchYaw. In other words, makes the RotationMatrix for a Spacefixed (extrinsic) XYZ rotation by “rollpitchyaw” angles
[r, p, y]
, which is equivalent to a Bodyfixed (intrinsic) ZYX rotation by “yawpitchroll” angles[y, p, r]
. Parameter
rpy
:  radian measures of three angles [roll, pitch, yaw].
 Parameter
rpy
:  a RollPitchYaw which is a Spacefixed (extrinsic) XYZ rotation
with “rollpitchyaw” angles
[r, p, y]
or equivalently a Body fixed (intrinsic) ZYX rotation with “yawpitchroll” angles[y, p, r]
.
Note
Denoting roll
r
, pitchp
, yawy
, this method returns a rotation matrixR_AD
equal to the matrix multiplication shown below.⎡cos(y) sin(y) 0⎤ ⎡ cos(p) 0 sin(p)⎤ ⎡1 0 0 ⎤ R_AD = ⎢sin(y) cos(y) 0⎥ * ⎢ 0 1 0 ⎥ * ⎢0 cos(r) sin(r)⎥ ⎣ 0 0 1⎦ ⎣sin(p) 0 cos(p)⎦ ⎣0 sin(r) cos(r)⎦ = R_AB * R_BC * R_CD
Note
In this discussion, A is the Space frame and D is the Body frame. One way to visualize this rotation sequence is by introducing intermediate frames B and C (useful constructs to understand this rotation sequence). Initially, the frames are aligned so
Di = Ci = Bi = Ai (i = x, y, z)
. Then D is subjected to successive righthanded rotations relative to A. 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a
roll angle
r
aboutDx = Cx
. Note: D and C are no longer aligned. 2nd rotation R_BC: Frames D, C (collectively – as if welded together)
rotate relative to frame B, A by a pitch angle
p
aboutCy = By
. Note: C and B are no longer aligned. 3rd rotation R_AB: Frames D, C, B (collectively – as if welded)
rotate relative to frame A by a roll angle
y
aboutBz = Az
. Note: B and A are no longer aligned.Note
This method constructs a RotationMatrix from a RollPitchYaw. Viceversa, there are highaccuracy RollPitchYaw constructor/methods that form a RollPitchYaw from a rotation matrix.

template
cast
¶ Instantiations:
cast[AutoDiffXd]

cast[AutoDiffXd]
(self: pydrake.math.RotationMatrix_[AutoDiffXd]) → pydrake.math.RotationMatrix_[AutoDiffXd]¶ Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T. For example,
RotationMatrix<double> source = RotationMatrix<double>::Identity(); RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>()
creates a newRotationMatrix<To>
from aRotationMatrix<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter


cast[AutoDiffXd]
(self: pydrake.math.RotationMatrix_[AutoDiffXd]) → pydrake.math.RotationMatrix_[AutoDiffXd]¶ Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T. For example,
RotationMatrix<double> source = RotationMatrix<double>::Identity(); RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>()
creates a newRotationMatrix<To>
from aRotationMatrix<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter

col
(self: pydrake.math.RotationMatrix_[AutoDiffXd], index: int) → numpy.ndarray[object[3, 1]]¶ Returns
this
rotation matrix’s iᵗʰ column (i = 0, 1, 2). Forthis
rotation matrix R_AB (which relates righthanded sets of orthogonal unit vectors Ax, Ay, Az to Bx, By, Bz),  col(0) returns Bx_A (Bx expressed in terms of Ax, Ay, Az).  col(1) returns By_A (By expressed in terms of Ax, Ay, Az).  col(2) returns Bz_A (Bz expressed in terms of Ax, Ay, Az). Parameter
index
:  requested column index (0 <= index <= 2).
See also
row(), matrix()
Raises: In debug builds, asserts (0 <= index <= 2). Note
For efficiency and consistency with Eigen, this method returns the same quantity returned by Eigen’s col() operator. The returned quantity can be assigned in various ways, e.g., as
const auto& Bz_A = col(2);
orVector3<T> Bz_A = col(2);
 Parameter

static
Identity
() → pydrake.math.RotationMatrix_[AutoDiffXd]¶

inverse
(self: pydrake.math.RotationMatrix_[AutoDiffXd]) → pydrake.math.RotationMatrix_[AutoDiffXd]¶

IsExactlyIdentity
(self: pydrake.math.RotationMatrix_[AutoDiffXd]) → bool¶ Returns
True
ifthis
is exactly equal to the identity matrix.

IsIdentityToInternalTolerance
(self: pydrake.math.RotationMatrix_[AutoDiffXd]) → bool¶ Returns true if
this
is equal to the identity matrix to within the threshold of get_internal_tolerance_for_orthonormality().

IsValid
(self: pydrake.math.RotationMatrix_[AutoDiffXd]) → bool¶ Tests if
this
rotation matrix R is a proper orthonormal rotation matrix to within the threshold of get_internal_tolerance_for_orthonormality().Returns: True
ifthis
is a valid rotation matrix.

static
MakeXRotation
(theta: pydrake.autodiffutils.AutoDiffXd) → pydrake.math.RotationMatrix_[AutoDiffXd]¶ Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAx = Bx
. Parameter
theta
:  radian measure of rotation angle about Ax.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitX().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a righthanded rotation relative to A by an angletheta
aboutAx = Bx
.⎡ 1 0 0 ⎤ R_AB = ⎢ 0 cos(theta) sin(theta) ⎥ ⎣ 0 sin(theta) cos(theta) ⎦
 Parameter

static
MakeYRotation
(theta: pydrake.autodiffutils.AutoDiffXd) → pydrake.math.RotationMatrix_[AutoDiffXd]¶ Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAy = By
. Parameter
theta
:  radian measure of rotation angle about Ay.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitY().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a righthanded rotation relative to A by an angletheta
aboutAy = By
.⎡ cos(theta) 0 sin(theta) ⎤ R_AB = ⎢ 0 1 0 ⎥ ⎣ sin(theta) 0 cos(theta) ⎦
 Parameter

static
MakeZRotation
(theta: pydrake.autodiffutils.AutoDiffXd) → pydrake.math.RotationMatrix_[AutoDiffXd]¶ Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAz = Bz
. Parameter
theta
:  radian measure of rotation angle about Az.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitZ().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a righthanded rotation relative to A by an angletheta
aboutAz = Bz
.⎡ cos(theta) sin(theta) 0 ⎤ R_AB = ⎢ sin(theta) cos(theta) 0 ⎥ ⎣ 0 0 1 ⎦
 Parameter

matrix
(self: pydrake.math.RotationMatrix_[AutoDiffXd]) → numpy.ndarray[object[3, 3]]¶ Returns the Matrix3 underlying a RotationMatrix.
See also
col(), row()

multiply
(*args, **kwargs)¶ Overloaded function.
 multiply(self: pydrake.math.RotationMatrix_[AutoDiffXd], other: pydrake.math.RotationMatrix_[AutoDiffXd]) > pydrake.math.RotationMatrix_[AutoDiffXd]
Calculates
this
rotation matrixR_AB
multiplied byother
rotation matrixR_BC
, returning the compositionR_AB * R_BC
. Parameter
other
:  RotationMatrix that postmultiplies
this
.
Returns: rotation matrix that results from this
multiplied byother
.Note
It is possible (albeit improbable) to create an invalid rotation matrix by accumulating roundoff error with a large number of multiplies.
 multiply(self: pydrake.math.RotationMatrix_[AutoDiffXd], v_B: numpy.ndarray[object[3, 1]]) > numpy.ndarray[object[3, 1]]
Calculates
this
rotation matrixR_AB
multiplied by an arbitrary Vector3 expressed in the B frame. Parameter
v_B
:  3x1 vector that postmultiplies
this
.
Returns: 3x1 vector v_A = R_AB * v_B
. multiply(self: pydrake.math.RotationMatrix_[AutoDiffXd], v_B: numpy.ndarray[object[3, n]]) > numpy.ndarray[object[3, n]]
Multiplies
this
RotationMatrixR_AB
by the n vectorsv1
, … vn, where each vector has 3 elements and is expressed in frame B. Parameter
v_B
: 3 x n
matrix whose n columns are regarded as arbitrary vectorsv1
, … vn expressed in frame B. Returns
v_A
: 3 x n
matrix whose n columns are vectorsv1
, … vn expressed in frame A.
const RollPitchYaw<double> rpy(0.1, 0.2, 0.3); const RotationMatrix<double> R_AB(rpy); Eigen::Matrix<double, 3, 2> v_B; v_B.col(0) = Vector3d(4, 5, 6); v_B.col(1) = Vector3d(9, 8, 7); const Eigen::Matrix<double, 3, 2> v_A = R_AB * v_B;

static
ProjectToRotationMatrix
(M: numpy.ndarray[object[3, 3]]) → pydrake.math.RotationMatrix_[AutoDiffXd]¶ Given an approximate rotation matrix M, finds the RotationMatrix R closest to M. Closeness is measured with a matrix2 norm (or equivalently with a Frobenius norm). Hence, this method creates a RotationMatrix R from a 3x3 matrix M by minimizing
‖R  M‖₂
(the matrix2 norm of (RM)) subject toR * Rᵀ = I
, where I is the 3x3 identity matrix. For this problem, closeness can also be measured by forming the orthonormal matrix R whose elements minimize the doublesummation∑ᵢ ∑ⱼ (R(i,j)  M(i,j))²
wherei = 1:3, j = 1:3
, subject toR * Rᵀ = I
. The squareroot of this doublesummation is called the Frobenius norm. Parameter
M
:  a 3x3 matrix.
 Parameter
quality_factor
:  . The quality of M as a rotation matrix.
quality_factor
= 1 is perfect (M = R).quality_factor
= 1.25 means that when M multiplies a unit vector (magnitude 1), a vector of magnitude as large as 1.25 may result.quality_factor
= 0.8 means that when M multiplies a unit vector, a vector of magnitude as small as 0.8 may result.quality_factor
= 0 means M is singular, so at least one of the bases related by matrix M does not span 3D space (when M multiples a unit vector, a vector of magnitude as small as 0 may result).
Returns: proper orthonormal matrix R that is closest to M. Raises: RuntimeError if R fails IsValid(R). Note
William Kahan (UC Berkeley) and Hongkai Dai (Toyota Research Institute) proved that for this problem, the same R that minimizes the Frobenius norm also minimizes the matrix2 norm (a.k.a an induced2 norm), which is defined [Dahleh, Section 4.2] as the column matrix u which maximizes
‖(R  M) u‖ / ‖u‖
, whereu ≠ 0
. Since the matrix2 norm of any matrix A is equal to the maximum singular value of A, minimizing the matrix2 norm of (R  M) is equivalent to minimizing the maximum singular value of (R  M). [Dahleh] “Lectures on Dynamic Systems and Controls: Electrical
Engineering and Computer Science, Massachusetts Institute of Technology” https://ocw.mit.edu/courses/electricalengineeringandcomputerscience/6241jdynamicsystemsandcontrolspring2011/readings/MIT6_241JS11_chap04.pdf
 Parameter

row
(self: pydrake.math.RotationMatrix_[AutoDiffXd], index: int) → numpy.ndarray[object[1, 3]]¶ Returns
this
rotation matrix’s iᵗʰ row (i = 0, 1, 2). Forthis
rotation matrix R_AB (which relates righthanded sets of orthogonal unit vectors Ax, Ay, Az to Bx, By, Bz),  row(0) returns Ax_B (Ax expressed in terms of Bx, By, Bz).  row(1) returns Ay_B (Ay expressed in terms of Bx, By, Bz).  row(2) returns Az_B (Az expressed in terms of Bx, By, Bz). Parameter
index
:  requested row index (0 <= index <= 2).
See also
col(), matrix()
Raises: In debug builds, asserts (0 <= index <= 2). Note
For efficiency and consistency with Eigen, this method returns the same quantity returned by Eigen’s row() operator. The returned quantity can be assigned in various ways, e.g., as
const auto& Az_B = row(2);
orRowVector3<T> Az_B = row(2);
 Parameter

set
(self: pydrake.math.RotationMatrix_[AutoDiffXd], R: numpy.ndarray[object[3, 3]]) → None¶ Sets
this
RotationMatrix from a Matrix3. Parameter
R
:  an allegedly valid rotation matrix.
Raises: RuntimeError in debug builds if R fails IsValid(R).  Parameter

ToAngleAxis
(self: pydrake.math.RotationMatrix_[AutoDiffXd]) → pydrake.common.eigen_geometry.AngleAxis_[AutoDiffXd]¶ Returns an AngleAxis
theta_lambda
containing an angletheta
and unit vector (axis direction)lambda
that representsthis
RotationMatrix.Note
The orientation and RotationMatrix associated with
theta * lambda
is identical to that of(theta) * (lambda)
. The AngleAxis returned by this method chooses to have0 <= theta <= pi
.Returns: an AngleAxis with 0 <= theta <= pi
and a unit vectorlambda
.

ToQuaternion
(self: pydrake.math.RotationMatrix_[AutoDiffXd]) → pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd]¶ Returns a quaternion q that represents
this
RotationMatrix. Since the quaternionq
andq
represent the same RotationMatrix, this method chooses to return a canonical quaternion, i.e., with q(0) >= 0.Note
There is a constructor in the RollPitchYaw class that converts a rotation matrix to rollpitchyaw angles.

transpose
(self: pydrake.math.RotationMatrix_[AutoDiffXd]) → pydrake.math.RotationMatrix_[AutoDiffXd]¶

class
pydrake.math.
RotationMatrix_[Expression]
¶ This class represents a 3x3 rotation matrix between two arbitrary frames A and B and helps ensure users create valid rotation matrices. This class relates righthanded orthogonal unit vectors Ax, Ay, Az fixed in frame A to righthanded orthogonal unit vectors Bx, By, Bz fixed in frame B. The monogram notation for the rotation matrix relating A to B is
R_AB
. An example that gives context to this rotation matrix isv_A = R_AB * v_B
, wherev_B
denotes an arbitrary vector v expressed in terms of Bx, By, Bz andv_A
denotes vector v expressed in terms of Ax, Ay, Az. See multibody_quantities for monogram notation for dynamics. See orientation_discussion “a discussion on rotation matrices”.Note
This class does not store the frames associated with a rotation matrix nor does it enforce strict proper usage of this class with vectors.
Note
When assertions are enabled, several methods in this class do a validity check and throw an exception (RuntimeError) if the rotation matrix is invalid. When assertions are disabled, many of these validity checks are skipped (which helps improve speed). In addition, these validity tests are only performed for scalar types for which drake::scalar_predicate<T>::is_bool is
True
. For instance, validity checks are not performed when T is symbolic::Expression. Authors:
 Paul Mitiguy (2018) Original author.
 Authors:
 Drake team (see https://drake.mit.edu/credits).

__init__
(*args, **kwargs)¶ Overloaded function.
 __init__(self: pydrake.math.RotationMatrix_[Expression]) > None
Constructs a 3x3 identity RotationMatrix – which corresponds to aligning two frames (so that unit vectors Ax = Bx, Ay = By, Az = Bz).
 __init__(self: pydrake.math.RotationMatrix_[Expression], other: pydrake.math.RotationMatrix_[Expression]) > None
 __init__(self: pydrake.math.RotationMatrix_[Expression], R: numpy.ndarray[object[3, 3]]) > None
Constructs a RotationMatrix from a Matrix3.
 Parameter
R
:  an allegedly valid rotation matrix.
Raises: RuntimeError in debug builds if R fails IsValid(R).  __init__(self: pydrake.math.RotationMatrix_[Expression], quaternion: pydrake.common.eigen_geometry.Quaternion_[Expression]) > None
Constructs a RotationMatrix from an Eigen::Quaternion.
 Parameter
quaternion
:  a nonzero, finite quaternion which may or may not have unit
length [i.e.,
quaternion.norm()
does not have to be 1].
Raises:  RuntimeError in debug builds if the rotation matrix R that is
 built from
quaternion
fails IsValid(R). For example, an  exception is thrown if
quaternion
is zero or contains a NaN or infinity.
Note
This method has the effect of normalizing its
quaternion
argument, without the inefficiency of the squareroot associated with normalization. __init__(self: pydrake.math.RotationMatrix_[Expression], theta_lambda: pydrake.common.eigen_geometry.AngleAxis_[Expression]) > None
Constructs a RotationMatrix from an Eigen::AngleAxis.
 Parameter
theta_lambda
:  an Eigen::AngleAxis whose associated axis (vector direction herein
called
lambda
) is nonzero and finite, but which may or may not have unit length [i.e.,lambda.norm()
does not have to be 1].
Raises:  RuntimeError in debug builds if the rotation matrix R that is
 built from
theta_lambda
fails IsValid(R). For example, an  exception is thrown if
lambda
is zero or contains a NaN or infinity.
 __init__(self: pydrake.math.RotationMatrix_[Expression], rpy: drake::math::RollPitchYaw<drake::symbolic::Expression>) > None
Constructs a RotationMatrix from an RollPitchYaw. In other words, makes the RotationMatrix for a Spacefixed (extrinsic) XYZ rotation by “rollpitchyaw” angles
[r, p, y]
, which is equivalent to a Bodyfixed (intrinsic) ZYX rotation by “yawpitchroll” angles[y, p, r]
. Parameter
rpy
:  radian measures of three angles [roll, pitch, yaw].
 Parameter
rpy
:  a RollPitchYaw which is a Spacefixed (extrinsic) XYZ rotation
with “rollpitchyaw” angles
[r, p, y]
or equivalently a Body fixed (intrinsic) ZYX rotation with “yawpitchroll” angles[y, p, r]
.
Note
Denoting roll
r
, pitchp
, yawy
, this method returns a rotation matrixR_AD
equal to the matrix multiplication shown below.⎡cos(y) sin(y) 0⎤ ⎡ cos(p) 0 sin(p)⎤ ⎡1 0 0 ⎤ R_AD = ⎢sin(y) cos(y) 0⎥ * ⎢ 0 1 0 ⎥ * ⎢0 cos(r) sin(r)⎥ ⎣ 0 0 1⎦ ⎣sin(p) 0 cos(p)⎦ ⎣0 sin(r) cos(r)⎦ = R_AB * R_BC * R_CD
Note
In this discussion, A is the Space frame and D is the Body frame. One way to visualize this rotation sequence is by introducing intermediate frames B and C (useful constructs to understand this rotation sequence). Initially, the frames are aligned so
Di = Ci = Bi = Ai (i = x, y, z)
. Then D is subjected to successive righthanded rotations relative to A. 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a
roll angle
r
aboutDx = Cx
. Note: D and C are no longer aligned. 2nd rotation R_BC: Frames D, C (collectively – as if welded together)
rotate relative to frame B, A by a pitch angle
p
aboutCy = By
. Note: C and B are no longer aligned. 3rd rotation R_AB: Frames D, C, B (collectively – as if welded)
rotate relative to frame A by a roll angle
y
aboutBz = Az
. Note: B and A are no longer aligned.Note
This method constructs a RotationMatrix from a RollPitchYaw. Viceversa, there are highaccuracy RollPitchYaw constructor/methods that form a RollPitchYaw from a rotation matrix.

template
cast
¶ Instantiations:
cast[Expression]

cast[Expression]
(self: pydrake.math.RotationMatrix_[Expression]) → pydrake.math.RotationMatrix_[Expression]¶ Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T. For example,
RotationMatrix<double> source = RotationMatrix<double>::Identity(); RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>()
creates a newRotationMatrix<To>
from aRotationMatrix<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter


cast[Expression]
(self: pydrake.math.RotationMatrix_[Expression]) → pydrake.math.RotationMatrix_[Expression]¶ Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T. For example,
RotationMatrix<double> source = RotationMatrix<double>::Identity(); RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>()
creates a newRotationMatrix<To>
from aRotationMatrix<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter

col
(self: pydrake.math.RotationMatrix_[Expression], index: int) → numpy.ndarray[object[3, 1]]¶ Returns
this
rotation matrix’s iᵗʰ column (i = 0, 1, 2). Forthis
rotation matrix R_AB (which relates righthanded sets of orthogonal unit vectors Ax, Ay, Az to Bx, By, Bz),  col(0) returns Bx_A (Bx expressed in terms of Ax, Ay, Az).  col(1) returns By_A (By expressed in terms of Ax, Ay, Az).  col(2) returns Bz_A (Bz expressed in terms of Ax, Ay, Az). Parameter
index
:  requested column index (0 <= index <= 2).
See also
row(), matrix()
Raises: In debug builds, asserts (0 <= index <= 2). Note
For efficiency and consistency with Eigen, this method returns the same quantity returned by Eigen’s col() operator. The returned quantity can be assigned in various ways, e.g., as
const auto& Bz_A = col(2);
orVector3<T> Bz_A = col(2);
 Parameter

static
Identity
() → pydrake.math.RotationMatrix_[Expression]¶

inverse
(self: pydrake.math.RotationMatrix_[Expression]) → pydrake.math.RotationMatrix_[Expression]¶

IsExactlyIdentity
(self: pydrake.math.RotationMatrix_[Expression]) → pydrake.symbolic.Formula¶ Returns
True
ifthis
is exactly equal to the identity matrix.

IsIdentityToInternalTolerance
(self: pydrake.math.RotationMatrix_[Expression]) → pydrake.symbolic.Formula¶ Returns true if
this
is equal to the identity matrix to within the threshold of get_internal_tolerance_for_orthonormality().

IsValid
(self: pydrake.math.RotationMatrix_[Expression]) → pydrake.symbolic.Formula¶ Tests if
this
rotation matrix R is a proper orthonormal rotation matrix to within the threshold of get_internal_tolerance_for_orthonormality().Returns: True
ifthis
is a valid rotation matrix.

static
MakeXRotation
(theta: pydrake.symbolic.Expression) → pydrake.math.RotationMatrix_[Expression]¶ Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAx = Bx
. Parameter
theta
:  radian measure of rotation angle about Ax.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitX().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a righthanded rotation relative to A by an angletheta
aboutAx = Bx
.⎡ 1 0 0 ⎤ R_AB = ⎢ 0 cos(theta) sin(theta) ⎥ ⎣ 0 sin(theta) cos(theta) ⎦
 Parameter

static
MakeYRotation
(theta: pydrake.symbolic.Expression) → pydrake.math.RotationMatrix_[Expression]¶ Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAy = By
. Parameter
theta
:  radian measure of rotation angle about Ay.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitY().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a righthanded rotation relative to A by an angletheta
aboutAy = By
.⎡ cos(theta) 0 sin(theta) ⎤ R_AB = ⎢ 0 1 0 ⎥ ⎣ sin(theta) 0 cos(theta) ⎦
 Parameter

static
MakeZRotation
(theta: pydrake.symbolic.Expression) → pydrake.math.RotationMatrix_[Expression]¶ Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAz = Bz
. Parameter
theta
:  radian measure of rotation angle about Az.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitZ().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a righthanded rotation relative to A by an angletheta
aboutAz = Bz
.⎡ cos(theta) sin(theta) 0 ⎤ R_AB = ⎢ sin(theta) cos(theta) 0 ⎥ ⎣ 0 0 1 ⎦
 Parameter

matrix
(self: pydrake.math.RotationMatrix_[Expression]) → numpy.ndarray[object[3, 3]]¶ Returns the Matrix3 underlying a RotationMatrix.
See also
col(), row()

multiply
(*args, **kwargs)¶ Overloaded function.
 multiply(self: pydrake.math.RotationMatrix_[Expression], other: pydrake.math.RotationMatrix_[Expression]) > pydrake.math.RotationMatrix_[Expression]
Calculates
this
rotation matrixR_AB
multiplied byother
rotation matrixR_BC
, returning the compositionR_AB * R_BC
. Parameter
other
:  RotationMatrix that postmultiplies
this
.
Returns: rotation matrix that results from this
multiplied byother
.Note
It is possible (albeit improbable) to create an invalid rotation matrix by accumulating roundoff error with a large number of multiplies.
 multiply(self: pydrake.math.RotationMatrix_[Expression], v_B: numpy.ndarray[object[3, 1]]) > numpy.ndarray[object[3, 1]]
Calculates
this
rotation matrixR_AB
multiplied by an arbitrary Vector3 expressed in the B frame. Parameter
v_B
:  3x1 vector that postmultiplies
this
.
Returns: 3x1 vector v_A = R_AB * v_B
. multiply(self: pydrake.math.RotationMatrix_[Expression], v_B: numpy.ndarray[object[3, n]]) > numpy.ndarray[object[3, n]]
Multiplies
this
RotationMatrixR_AB
by the n vectorsv1
, … vn, where each vector has 3 elements and is expressed in frame B. Parameter
v_B
: 3 x n
matrix whose n columns are regarded as arbitrary vectorsv1
, … vn expressed in frame B. Returns
v_A
: 3 x n
matrix whose n columns are vectorsv1
, … vn expressed in frame A.
const RollPitchYaw<double> rpy(0.1, 0.2, 0.3); const RotationMatrix<double> R_AB(rpy); Eigen::Matrix<double, 3, 2> v_B; v_B.col(0) = Vector3d(4, 5, 6); v_B.col(1) = Vector3d(9, 8, 7); const Eigen::Matrix<double, 3, 2> v_A = R_AB * v_B;

static
ProjectToRotationMatrix
(M: numpy.ndarray[object[3, 3]]) → pydrake.math.RotationMatrix_[Expression]¶ Given an approximate rotation matrix M, finds the RotationMatrix R closest to M. Closeness is measured with a matrix2 norm (or equivalently with a Frobenius norm). Hence, this method creates a RotationMatrix R from a 3x3 matrix M by minimizing
‖R  M‖₂
(the matrix2 norm of (RM)) subject toR * Rᵀ = I
, where I is the 3x3 identity matrix. For this problem, closeness can also be measured by forming the orthonormal matrix R whose elements minimize the doublesummation∑ᵢ ∑ⱼ (R(i,j)  M(i,j))²
wherei = 1:3, j = 1:3
, subject toR * Rᵀ = I
. The squareroot of this doublesummation is called the Frobenius norm. Parameter
M
:  a 3x3 matrix.
 Parameter
quality_factor
:  . The quality of M as a rotation matrix.
quality_factor
= 1 is perfect (M = R).quality_factor
= 1.25 means that when M multiplies a unit vector (magnitude 1), a vector of magnitude as large as 1.25 may result.quality_factor
= 0.8 means that when M multiplies a unit vector, a vector of magnitude as small as 0.8 may result.quality_factor
= 0 means M is singular, so at least one of the bases related by matrix M does not span 3D space (when M multiples a unit vector, a vector of magnitude as small as 0 may result).
Returns: proper orthonormal matrix R that is closest to M. Raises: RuntimeError if R fails IsValid(R). Note
William Kahan (UC Berkeley) and Hongkai Dai (Toyota Research Institute) proved that for this problem, the same R that minimizes the Frobenius norm also minimizes the matrix2 norm (a.k.a an induced2 norm), which is defined [Dahleh, Section 4.2] as the column matrix u which maximizes
‖(R  M) u‖ / ‖u‖
, whereu ≠ 0
. Since the matrix2 norm of any matrix A is equal to the maximum singular value of A, minimizing the matrix2 norm of (R  M) is equivalent to minimizing the maximum singular value of (R  M). [Dahleh] “Lectures on Dynamic Systems and Controls: Electrical
Engineering and Computer Science, Massachusetts Institute of Technology” https://ocw.mit.edu/courses/electricalengineeringandcomputerscience/6241jdynamicsystemsandcontrolspring2011/readings/MIT6_241JS11_chap04.pdf
 Parameter

row
(self: pydrake.math.RotationMatrix_[Expression], index: int) → numpy.ndarray[object[1, 3]]¶ Returns
this
rotation matrix’s iᵗʰ row (i = 0, 1, 2). Forthis
rotation matrix R_AB (which relates righthanded sets of orthogonal unit vectors Ax, Ay, Az to Bx, By, Bz),  row(0) returns Ax_B (Ax expressed in terms of Bx, By, Bz).  row(1) returns Ay_B (Ay expressed in terms of Bx, By, Bz).  row(2) returns Az_B (Az expressed in terms of Bx, By, Bz). Parameter
index
:  requested row index (0 <= index <= 2).
See also
col(), matrix()
Raises: In debug builds, asserts (0 <= index <= 2). Note
For efficiency and consistency with Eigen, this method returns the same quantity returned by Eigen’s row() operator. The returned quantity can be assigned in various ways, e.g., as
const auto& Az_B = row(2);
orRowVector3<T> Az_B = row(2);
 Parameter

set
(self: pydrake.math.RotationMatrix_[Expression], R: numpy.ndarray[object[3, 3]]) → None¶ Sets
this
RotationMatrix from a Matrix3. Parameter
R
:  an allegedly valid rotation matrix.
Raises: RuntimeError in debug builds if R fails IsValid(R).  Parameter

ToAngleAxis
(self: pydrake.math.RotationMatrix_[Expression]) → pydrake.common.eigen_geometry.AngleAxis_[Expression]¶ Returns an AngleAxis
theta_lambda
containing an angletheta
and unit vector (axis direction)lambda
that representsthis
RotationMatrix.Note
The orientation and RotationMatrix associated with
theta * lambda
is identical to that of(theta) * (lambda)
. The AngleAxis returned by this method chooses to have0 <= theta <= pi
.Returns: an AngleAxis with 0 <= theta <= pi
and a unit vectorlambda
.

ToQuaternion
(self: pydrake.math.RotationMatrix_[Expression]) → pydrake.common.eigen_geometry.Quaternion_[Expression]¶ Returns a quaternion q that represents
this
RotationMatrix. Since the quaternionq
andq
represent the same RotationMatrix, this method chooses to return a canonical quaternion, i.e., with q(0) >= 0.Note
There is a constructor in the RollPitchYaw class that converts a rotation matrix to rollpitchyaw angles.

transpose
(self: pydrake.math.RotationMatrix_[Expression]) → pydrake.math.RotationMatrix_[Expression]¶

class
pydrake.math.
RotationMatrix_[float]
¶ This class represents a 3x3 rotation matrix between two arbitrary frames A and B and helps ensure users create valid rotation matrices. This class relates righthanded orthogonal unit vectors Ax, Ay, Az fixed in frame A to righthanded orthogonal unit vectors Bx, By, Bz fixed in frame B. The monogram notation for the rotation matrix relating A to B is
R_AB
. An example that gives context to this rotation matrix isv_A = R_AB * v_B
, wherev_B
denotes an arbitrary vector v expressed in terms of Bx, By, Bz andv_A
denotes vector v expressed in terms of Ax, Ay, Az. See multibody_quantities for monogram notation for dynamics. See orientation_discussion “a discussion on rotation matrices”.Note
This class does not store the frames associated with a rotation matrix nor does it enforce strict proper usage of this class with vectors.
Note
When assertions are enabled, several methods in this class do a validity check and throw an exception (RuntimeError) if the rotation matrix is invalid. When assertions are disabled, many of these validity checks are skipped (which helps improve speed). In addition, these validity tests are only performed for scalar types for which drake::scalar_predicate<T>::is_bool is
True
. For instance, validity checks are not performed when T is symbolic::Expression. Authors:
 Paul Mitiguy (2018) Original author.
 Authors:
 Drake team (see https://drake.mit.edu/credits).

__init__
(*args, **kwargs)¶ Overloaded function.
 __init__(self: pydrake.math.RotationMatrix_[float]) > None
Constructs a 3x3 identity RotationMatrix – which corresponds to aligning two frames (so that unit vectors Ax = Bx, Ay = By, Az = Bz).
 __init__(self: pydrake.math.RotationMatrix_[float], other: pydrake.math.RotationMatrix_[float]) > None
 __init__(self: pydrake.math.RotationMatrix_[float], R: numpy.ndarray[float64[3, 3]]) > None
Constructs a RotationMatrix from a Matrix3.
 Parameter
R
:  an allegedly valid rotation matrix.
Raises: RuntimeError in debug builds if R fails IsValid(R).  __init__(self: pydrake.math.RotationMatrix_[float], quaternion: Eigen::Quaternion<double, 0>) > None
Constructs a RotationMatrix from an Eigen::Quaternion.
 Parameter
quaternion
:  a nonzero, finite quaternion which may or may not have unit
length [i.e.,
quaternion.norm()
does not have to be 1].
Raises:  RuntimeError in debug builds if the rotation matrix R that is
 built from
quaternion
fails IsValid(R). For example, an  exception is thrown if
quaternion
is zero or contains a NaN or infinity.
Note
This method has the effect of normalizing its
quaternion
argument, without the inefficiency of the squareroot associated with normalization. __init__(self: pydrake.math.RotationMatrix_[float], theta_lambda: Eigen::AngleAxis<double>) > None
Constructs a RotationMatrix from an Eigen::AngleAxis.
 Parameter
theta_lambda
:  an Eigen::AngleAxis whose associated axis (vector direction herein
called
lambda
) is nonzero and finite, but which may or may not have unit length [i.e.,lambda.norm()
does not have to be 1].
Raises:  RuntimeError in debug builds if the rotation matrix R that is
 built from
theta_lambda
fails IsValid(R). For example, an  exception is thrown if
lambda
is zero or contains a NaN or infinity.
 __init__(self: pydrake.math.RotationMatrix_[float], rpy: drake::math::RollPitchYaw<double>) > None
Constructs a RotationMatrix from an RollPitchYaw. In other words, makes the RotationMatrix for a Spacefixed (extrinsic) XYZ rotation by “rollpitchyaw” angles
[r, p, y]
, which is equivalent to a Bodyfixed (intrinsic) ZYX rotation by “yawpitchroll” angles[y, p, r]
. Parameter
rpy
:  radian measures of three angles [roll, pitch, yaw].
 Parameter
rpy
:  a RollPitchYaw which is a Spacefixed (extrinsic) XYZ rotation
with “rollpitchyaw” angles
[r, p, y]
or equivalently a Body fixed (intrinsic) ZYX rotation with “yawpitchroll” angles[y, p, r]
.
Note
Denoting roll
r
, pitchp
, yawy
, this method returns a rotation matrixR_AD
equal to the matrix multiplication shown below.⎡cos(y) sin(y) 0⎤ ⎡ cos(p) 0 sin(p)⎤ ⎡1 0 0 ⎤ R_AD = ⎢sin(y) cos(y) 0⎥ * ⎢ 0 1 0 ⎥ * ⎢0 cos(r) sin(r)⎥ ⎣ 0 0 1⎦ ⎣sin(p) 0 cos(p)⎦ ⎣0 sin(r) cos(r)⎦ = R_AB * R_BC * R_CD
Note
In this discussion, A is the Space frame and D is the Body frame. One way to visualize this rotation sequence is by introducing intermediate frames B and C (useful constructs to understand this rotation sequence). Initially, the frames are aligned so
Di = Ci = Bi = Ai (i = x, y, z)
. Then D is subjected to successive righthanded rotations relative to A. 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a
roll angle
r
aboutDx = Cx
. Note: D and C are no longer aligned. 2nd rotation R_BC: Frames D, C (collectively – as if welded together)
rotate relative to frame B, A by a pitch angle
p
aboutCy = By
. Note: C and B are no longer aligned. 3rd rotation R_AB: Frames D, C, B (collectively – as if welded)
rotate relative to frame A by a roll angle
y
aboutBz = Az
. Note: B and A are no longer aligned.Note
This method constructs a RotationMatrix from a RollPitchYaw. Viceversa, there are highaccuracy RollPitchYaw constructor/methods that form a RollPitchYaw from a rotation matrix.

template
cast
¶ Instantiations:
cast[float]
,cast[AutoDiffXd]
,cast[Expression]

cast[float]
(self: pydrake.math.RotationMatrix_[float]) → pydrake.math.RotationMatrix_[float]¶ Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T. For example,
RotationMatrix<double> source = RotationMatrix<double>::Identity(); RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>()
creates a newRotationMatrix<To>
from aRotationMatrix<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter


cast[AutoDiffXd]
(self: pydrake.math.RotationMatrix_[float]) → drake::math::RotationMatrix<Eigen::AutoDiffScalar<Eigen::Matrix<double, 1, 1, 0, 1, 1> > >¶ Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T. For example,
RotationMatrix<double> source = RotationMatrix<double>::Identity(); RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>()
creates a newRotationMatrix<To>
from aRotationMatrix<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter

cast[Expression]
(self: pydrake.math.RotationMatrix_[float]) → drake::math::RotationMatrix<drake::symbolic::Expression>¶ Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T. For example,
RotationMatrix<double> source = RotationMatrix<double>::Identity(); RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>()
creates a newRotationMatrix<To>
from aRotationMatrix<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter

cast[float]
(self: pydrake.math.RotationMatrix_[float]) → pydrake.math.RotationMatrix_[float]¶ Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T. For example,
RotationMatrix<double> source = RotationMatrix<double>::Identity(); RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
 Template parameter
U
:  Scalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>()
creates a newRotationMatrix<To>
from aRotationMatrix<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not viceversa. Template parameter

col
(self: pydrake.math.RotationMatrix_[float], index: int) → numpy.ndarray[float64[3, 1]]¶ Returns
this
rotation matrix’s iᵗʰ column (i = 0, 1, 2). Forthis
rotation matrix R_AB (which relates righthanded sets of orthogonal unit vectors Ax, Ay, Az to Bx, By, Bz),  col(0) returns Bx_A (Bx expressed in terms of Ax, Ay, Az).  col(1) returns By_A (By expressed in terms of Ax, Ay, Az).  col(2) returns Bz_A (Bz expressed in terms of Ax, Ay, Az). Parameter
index
:  requested column index (0 <= index <= 2).
See also
row(), matrix()
Raises: In debug builds, asserts (0 <= index <= 2). Note
For efficiency and consistency with Eigen, this method returns the same quantity returned by Eigen’s col() operator. The returned quantity can be assigned in various ways, e.g., as
const auto& Bz_A = col(2);
orVector3<T> Bz_A = col(2);
 Parameter

static
Identity
() → pydrake.math.RotationMatrix_[float]¶

inverse
(self: pydrake.math.RotationMatrix_[float]) → pydrake.math.RotationMatrix_[float]¶

IsExactlyIdentity
(self: pydrake.math.RotationMatrix_[float]) → bool¶ Returns
True
ifthis
is exactly equal to the identity matrix.

IsIdentityToInternalTolerance
(self: pydrake.math.RotationMatrix_[float]) → bool¶ Returns true if
this
is equal to the identity matrix to within the threshold of get_internal_tolerance_for_orthonormality().

IsValid
(self: pydrake.math.RotationMatrix_[float]) → bool¶ Tests if
this
rotation matrix R is a proper orthonormal rotation matrix to within the threshold of get_internal_tolerance_for_orthonormality().Returns: True
ifthis
is a valid rotation matrix.

static
MakeXRotation
(theta: float) → pydrake.math.RotationMatrix_[float]¶ Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAx = Bx
. Parameter
theta
:  radian measure of rotation angle about Ax.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitX().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a righthanded rotation relative to A by an angletheta
aboutAx = Bx
.⎡ 1 0 0 ⎤ R_AB = ⎢ 0 cos(theta) sin(theta) ⎥ ⎣ 0 sin(theta) cos(theta) ⎦
 Parameter

static
MakeYRotation
(theta: float) → pydrake.math.RotationMatrix_[float]¶ Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAy = By
. Parameter
theta
:  radian measure of rotation angle about Ay.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitY().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a righthanded rotation relative to A by an angletheta
aboutAy = By
.⎡ cos(theta) 0 sin(theta) ⎤ R_AB = ⎢ 0 1 0 ⎥ ⎣ sin(theta) 0 cos(theta) ⎦
 Parameter

static
MakeZRotation
(theta: float) → pydrake.math.RotationMatrix_[float]¶ Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAz = Bz
. Parameter
theta
:  radian measure of rotation angle about Az.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitZ().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a righthanded rotation relative to A by an angletheta
aboutAz = Bz
.⎡ cos(theta) sin(theta) 0 ⎤ R_AB = ⎢ sin(theta) cos(theta) 0 ⎥ ⎣ 0 0 1 ⎦
 Parameter

matrix
(self: pydrake.math.RotationMatrix_[float]) → numpy.ndarray[float64[3, 3]]¶ Returns the Matrix3 underlying a RotationMatrix.
See also
col(), row()

multiply
(*args, **kwargs)¶ Overloaded function.
 multiply(self: pydrake.math.RotationMatrix_[float], other: pydrake.math.RotationMatrix_[float]) > pydrake.math.RotationMatrix_[float]
Calculates
this
rotation matrixR_AB
multiplied byother
rotation matrixR_BC
, returning the compositionR_AB * R_BC
. Parameter
other
:  RotationMatrix that postmultiplies
this
.
Returns: rotation matrix that results from this
multiplied byother
.Note
It is possible (albeit improbable) to create an invalid rotation matrix by accumulating roundoff error with a large number of multiplies.
 multiply(self: pydrake.math.RotationMatrix_[float], v_B: numpy.ndarray[float64[3, 1]]) > numpy.ndarray[float64[3, 1]]
Calculates
this
rotation matrixR_AB
multiplied by an arbitrary Vector3 expressed in the B frame. Parameter
v_B
:  3x1 vector that postmultiplies
this
.
Returns: 3x1 vector v_A = R_AB * v_B
. multiply(self: pydrake.math.RotationMatrix_[float], v_B: numpy.ndarray[float64[3, n]]) > numpy.ndarray[float64[3, n]]
Multiplies
this
RotationMatrixR_AB
by the n vectorsv1
, … vn, where each vector has 3 elements and is expressed in frame B. Parameter
v_B
: 3 x n
matrix whose n columns are regarded as arbitrary vectorsv1
, … vn expressed in frame B. Returns
v_A
: 3 x n
matrix whose n columns are vectorsv1
, … vn expressed in frame A.
const RollPitchYaw<double> rpy(0.1, 0.2, 0.3); const RotationMatrix<double> R_AB(rpy); Eigen::Matrix<double, 3, 2> v_B; v_B.col(0) = Vector3d(4, 5, 6); v_B.col(1) = Vector3d(9, 8, 7); const Eigen::Matrix<double, 3, 2> v_A = R_AB * v_B;

static
ProjectToRotationMatrix
(M: numpy.ndarray[float64[3, 3]]) → pydrake.math.RotationMatrix_[float]¶ Given an approximate rotation matrix M, finds the RotationMatrix R closest to M. Closeness is measured with a matrix2 norm (or equivalently with a Frobenius norm). Hence, this method creates a RotationMatrix R from a 3x3 matrix M by minimizing
‖R  M‖₂
(the matrix2 norm of (RM)) subject toR * Rᵀ = I
, where I is the 3x3 identity matrix. For this problem, closeness can also be measured by forming the orthonormal matrix R whose elements minimize the doublesummation∑ᵢ ∑ⱼ (R(i,j)  M(i,j))²
wherei = 1:3, j = 1:3
, subject toR * Rᵀ = I
. The squareroot of this doublesummation is called the Frobenius norm. Parameter
M
:  a 3x3 matrix.
 Parameter
quality_factor
:  . The quality of M as a rotation matrix.
quality_factor
= 1 is perfect (M = R).quality_factor
= 1.25 means that when M multiplies a unit vector (magnitude 1), a vector of magnitude as large as 1.25 may result.quality_factor
= 0.8 means that when M multiplies a unit vector, a vector of magnitude as small as 0.8 may result.quality_factor
= 0 means M is singular, so at least one of the bases related by matrix M does not span 3D space (when M multiples a unit vector, a vector of magnitude as small as 0 may result).
Returns: proper orthonormal matrix R that is closest to M. Raises: RuntimeError if R fails IsValid(R). Note
William Kahan (UC Berkeley) and Hongkai Dai (Toyota Research Institute) proved that for this problem, the same R that minimizes the Frobenius norm also minimizes the matrix2 norm (a.k.a an induced2 norm), which is defined [Dahleh, Section 4.2] as the column matrix u which maximizes
‖(R  M) u‖ / ‖u‖
, whereu ≠ 0
. Since the matrix2 norm of any matrix A is equal to the maximum singular value of A, minimizing the matrix2 norm of (R  M) is equivalent to minimizing the maximum singular value of (R  M). [Dahleh] “Lectures on Dynamic Systems and Controls: Electrical
Engineering and Computer Science, Massachusetts Institute of Technology” https://ocw.mit.edu/courses/electricalengineeringandcomputerscience/6241jdynamicsystemsandcontrolspring2011/readings/MIT6_241JS11_chap04.pdf
 Parameter

row
(self: pydrake.math.RotationMatrix_[float], index: int) → numpy.ndarray[float64[1, 3]]¶ Returns
this
rotation matrix’s iᵗʰ row (i = 0, 1, 2). Forthis
rotation matrix R_AB (which relates righthanded sets of orthogonal unit vectors Ax, Ay, Az to Bx, By, Bz),  row(0) returns Ax_B (Ax expressed in terms of Bx, By, Bz).  row(1) returns Ay_B (Ay expressed in terms of Bx, By, Bz).  row(2) returns Az_B (Az expressed in terms of Bx, By, Bz). Parameter
index
:  requested row index (0 <= index <= 2).
See also
col(), matrix()
Raises: In debug builds, asserts (0 <= index <= 2). Note
For efficiency and consistency with Eigen, this method returns the same quantity returned by Eigen’s row() operator. The returned quantity can be assigned in various ways, e.g., as
const auto& Az_B = row(2);
orRowVector3<T> Az_B = row(2);
 Parameter

set
(self: pydrake.math.RotationMatrix_[float], R: numpy.ndarray[float64[3, 3]]) → None¶ Sets
this
RotationMatrix from a Matrix3. Parameter
R
:  an allegedly valid rotation matrix.
Raises: RuntimeError in debug builds if R fails IsValid(R).  Parameter

ToAngleAxis
(self: pydrake.math.RotationMatrix_[float]) → Eigen::AngleAxis<double>¶ Returns an AngleAxis
theta_lambda
containing an angletheta
and unit vector (axis direction)lambda
that representsthis
RotationMatrix.Note
The orientation and RotationMatrix associated with
theta * lambda
is identical to that of(theta) * (lambda)
. The AngleAxis returned by this method chooses to have0 <= theta <= pi
.Returns: an AngleAxis with 0 <= theta <= pi
and a unit vectorlambda
.

ToQuaternion
(self: pydrake.math.RotationMatrix_[float]) → Eigen::Quaternion<double, 0>¶ Returns a quaternion q that represents
this
RotationMatrix. Since the quaternionq
andq
represent the same RotationMatrix, this method chooses to return a canonical quaternion, i.e., with q(0) >= 0.Note
There is a constructor in the RollPitchYaw class that converts a rotation matrix to rollpitchyaw angles.

transpose
(self: pydrake.math.RotationMatrix_[float]) → pydrake.math.RotationMatrix_[float]¶

pydrake.math.
sin
(*args, **kwargs)¶ Overloaded function.
 sin(arg0: float) > float
 sin(arg0: pydrake.autodiffutils.AutoDiffXd) > pydrake.autodiffutils.AutoDiffXd
 sin(arg0: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
sinh
(*args, **kwargs)¶ Overloaded function.
 sinh(arg0: float) > float
 sinh(arg0: pydrake.autodiffutils.AutoDiffXd) > pydrake.autodiffutils.AutoDiffXd
 sinh(arg0: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
sqrt
(*args, **kwargs)¶ Overloaded function.
 sqrt(arg0: float) > float
 sqrt(arg0: pydrake.autodiffutils.AutoDiffXd) > pydrake.autodiffutils.AutoDiffXd
 sqrt(arg0: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
tan
(*args, **kwargs)¶ Overloaded function.
 tan(arg0: float) > float
 tan(arg0: pydrake.autodiffutils.AutoDiffXd) > pydrake.autodiffutils.AutoDiffXd
 tan(arg0: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
tanh
(*args, **kwargs)¶ Overloaded function.
 tanh(arg0: float) > float
 tanh(arg0: pydrake.autodiffutils.AutoDiffXd) > pydrake.autodiffutils.AutoDiffXd
 tanh(arg0: pydrake.symbolic.Expression) > pydrake.symbolic.Expression

pydrake.math.
wrap_to
(value: float, low: float, high: float) → float¶ 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*(highlow) for the unique integer valuek
that lands the output in the desired interval.low
andhigh
must be finite, and low < high.