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)
class pydrake.math.BarycentricMesh

Represents a multi-linear 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

Template parameter T:
The vector element type, which must be a valid Eigen scalar.

Instantiated templates for the following kinds of T’s are provided:

• double
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 to weights. 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().
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)); });
__init__(self: pydrake.math.BarycentricMesh, arg0: List[Set[float]]) → None

Constructs the mesh.

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).
get_num_interpolants(self: pydrake.math.BarycentricMesh) → int
get_num_mesh_points(self: pydrake.math.BarycentricMesh) → int
pydrake.math.ComputeBasisFromAxis(axis_index: int, axis_W: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 3]]

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

Parameter axis_index:
The index of the axis (in the range [0,2]), with 0 corresponding to the x-axis, 1 corresponding to the y-axis, and z-corresponding to the z-axis.
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 1e-10 to zero or axis_index does not lie in the range [0,2].
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 continuous-time algebraic Riccati equation:

S A + A' S - S B inv(R) 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.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.

1. zero_tol is non-negative.

$Raises: RuntimeError when the pre-conditions are not satisfied. 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 non-negative, 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 non-negative, namely the matrix

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

is positive semidefinite. 2. Q and b are of the correct size. 3. tol is non-negative.

Raises: RuntimeError if the precondition is not satisfied.
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 discrete-time algebraic Riccati equation:

$A'XA - X - A'XB(B'XB+R)^{-1}B'XA + Q = 0$
Raises: RuntimeError if Q is not positive semi-definite. RuntimeError if R is not positive definite.

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

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 semi-definite, then X is also positive semi-definite . Therefore, if one searches for a Lyapunov function V(z) = zᵀXz for the stable linear system ż = Az, then the solution of the Lyapunov Equation AᵀX + XA + Q = 0 only returns a valid Lyapunov function if Q is positive semi-definite.

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

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 1e-10. It has been used to check (1) if λᵢ + λ̅ⱼ = 0, ∀ i,j; (2) if A is a 1-by-1 zero matrix; (3) if A’s trace or determinant is 0 when A is a 2-by-2 matrix.

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

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 .

Raises: RuntimeError if the solution is not unique.

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

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

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 1e-10. It has been used to check (1) if λᵢ = ±1 ∀ i; (2) if λᵢ * λⱼ = 1, i ≠ j.

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

pydrake.math.RigidTransform
template pydrake.math.RigidTransform_
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 distance-preserving 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 right-handed orthogonal unit vectors Ax, Ay, Az fixed in frame A to right-handed 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 is R_AB. The monogram notation for the position vector from Ao to Bo is p_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 not X_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 (non-NAN) position vector. Eigen::Isometry does not. - RigidTransform catches bugs that are undetected by Eigen::Isometry. - RigidTransform has additional functionality and ease-of-use, 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 counter-intuitive 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).
Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.
static FromMatrix4(matrix: numpy.ndarray[float64[4, 4]]) → pydrake.math.RigidTransform_[float]

Please use RigidTransfrom(pose=...) constructor. This will be removed on or around 2019-11-15.

GetAsIsometry3(self: pydrake.math.RigidTransform_[float]) → pydrake.common.eigen_geometry.Isometry3_[float]

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.
SetFromIsometry3(self: pydrake.math.RigidTransform_[float], pose: pydrake.common.eigen_geometry.Isometry3_[float]) → 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 vector p_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().

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.

__init__(*args, **kwargs)

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

1. __init__(self: pydrake.math.RigidTransform_[float], other: pydrake.math.RigidTransform_[float]) -> None
2. __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.
1. __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 Space-fixed (extrinsic) X-Y-Z rotation with “roll-pitch-yaw” angles [r, p, y] or equivalently a Body- fixed (intrinsic) Z-Y-X rotation with “yaw-pitch-roll” angles [y, p, r].

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.
1. __init__(self: pydrake.math.RigidTransform_[float], quaternion: pydrake.common.eigen_geometry.Quaternion_[float], p: numpy.ndarray[float64[3, 1]]) -> None

Constructs a RigidTransform from a Quaternion and a position vector.

Parameter quaternion:
a non-zero, 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.

RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&)

1. __init__(self: pydrake.math.RigidTransform_[float], theta_lambda: pydrake.common.eigen_geometry.AngleAxis_[float], 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 non-zero 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.

RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&)

1. __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).
1. __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.
1. __init__(self: pydrake.math.RigidTransform_[float], pose: pydrake.common.eigen_geometry.Isometry3_[float]) -> 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 vector p_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().

1. __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);
1. __init__(self: pydrake.math.RigidTransform_[float], matrix: numpy.ndarray[float64[4, 4]]) -> None

Please use RigidTransfrom(pose=...) constructor. This will be removed on or around 2019-11-15.

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 new RigidTransform<To> from a RigidTransform<From> but only if type To is constructible from type From. 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 vice-versa.

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 new RigidTransform<To> from a RigidTransform<From> but only if type To is constructible from type From. 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 vice-versa.

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 new RigidTransform<To> from a RigidTransform<From> but only if type To is constructible from type From. 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 vice-versa.

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 new RigidTransform<To> from a RigidTransform<From> but only if type To is constructible from type From. 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 vice-versa.

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 square-root 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 2019-12-01. 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 2019-12-01. See drake issue #9865 for details.

multiply(*args, **kwargs)

1. multiply(self: pydrake.math.RigidTransform_[float], other: pydrake.math.RigidTransform_[float]) -> pydrake.math.RigidTransform_[float]

Multiplies this RigidTransform X_AB by the other RigidTransform X_BC and returns the RigidTransform X_AC = X_AB * X_BC.

1. multiply(self: pydrake.math.RigidTransform_[float], p_BoQ_B: numpy.ndarray[float64[3, 1]]) -> numpy.ndarray[float64[3, 1]]

Multiplies this RigidTransform X_AB by the position vector p_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.
1. multiply(self: pydrake.math.RigidTransform_[float], p_BoQ_B: numpy.ndarray[float64[3, n]]) -> numpy.ndarray[float64[3, n]]

Multiplies this RigidTransform X_AB by the n position vectors p_BoQ1_Bp_BoQn_B, where p_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 vectors p_BoQi_B or an expression that resolves to a 3 x n matrix of position vectors.
Returns p_AoQ_A:
3 x n matrix with n position vectors p_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 that X_AB * p_BoQ_B returns p_AoQ_A = p_AoBo_A + R_AB * p_BoQ_B, where p_AoBo_A is the position vector from Ao to Bo expressed in A and R_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.
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.
set_rotation(*args, **kwargs)

1. 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).
1. 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:
“roll-pitch-yaw” angles.

RotationMatrix::RotationMatrix(const RollPitchYaw<T>&) which describes the parameter, preconditions, etc.

1. set_rotation(self: pydrake.math.RigidTransform_[float], quaternion: pydrake.common.eigen_geometry.Quaternion_[float]) -> None

Sets the rotation part of this RigidTransform from a Quaternion.

Parameter quaternion:
a quaternion which may or may not have unit length.

RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&) which describes the parameter, preconditions, exception conditions, etc.

1. set_rotation(self: pydrake.math.RigidTransform_[float], theta_lambda: pydrake.common.eigen_geometry.AngleAxis_[float]) -> None

Sets the rotation part of this RigidTransform from an AngleAxis.

Parameter theta_lambda:
an angle theta (in radians) and vector lambda.

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.
translation(self: pydrake.math.RigidTransform_[float]) → numpy.ndarray[float64[3, 1]]

Returns p_AoBo_A, the position vector portion of this RigidTransform, i.e., position vector from Ao (frame A’s origin) to Bo (frame B’s origin).

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 distance-preserving 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 right-handed orthogonal unit vectors Ax, Ay, Az fixed in frame A to right-handed 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 is R_AB. The monogram notation for the position vector from Ao to Bo is p_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 not X_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 (non-NAN) position vector. Eigen::Isometry does not. - RigidTransform catches bugs that are undetected by Eigen::Isometry. - RigidTransform has additional functionality and ease-of-use, 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 counter-intuitive 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).
Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.
static FromMatrix4(matrix: numpy.ndarray[object[4, 4]]) → pydrake.math.RigidTransform_[AutoDiffXd]

Please use RigidTransfrom(pose=...) constructor. This will be removed on or around 2019-11-15.

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.
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 vector p_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().

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.

__init__(*args, **kwargs)

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

1. __init__(self: pydrake.math.RigidTransform_[AutoDiffXd], other: pydrake.math.RigidTransform_[AutoDiffXd]) -> None
2. __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.
1. __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 Space-fixed (extrinsic) X-Y-Z rotation with “roll-pitch-yaw” angles [r, p, y] or equivalently a Body- fixed (intrinsic) Z-Y-X rotation with “yaw-pitch-roll” angles [y, p, r].

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.
1. __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 non-zero, 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.

RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&)

1. __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 non-zero 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.

RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&)

1. __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).
1. __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.
1. __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 vector p_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().

1. __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);
1. __init__(self: pydrake.math.RigidTransform_[AutoDiffXd], matrix: numpy.ndarray[object[4, 4]]) -> None

Please use RigidTransfrom(pose=...) constructor. This will be removed on or around 2019-11-15.

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 new RigidTransform<To> from a RigidTransform<From> but only if type To is constructible from type From. 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 vice-versa.

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 new RigidTransform<To> from a RigidTransform<From> but only if type To is constructible from type From. 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 vice-versa.

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 square-root 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 2019-12-01. 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 2019-12-01. See drake issue #9865 for details.

multiply(*args, **kwargs)

1. multiply(self: pydrake.math.RigidTransform_[AutoDiffXd], other: pydrake.math.RigidTransform_[AutoDiffXd]) -> pydrake.math.RigidTransform_[AutoDiffXd]

Multiplies this RigidTransform X_AB by the other RigidTransform X_BC and returns the RigidTransform X_AC = X_AB * X_BC.

1. multiply(self: pydrake.math.RigidTransform_[AutoDiffXd], p_BoQ_B: numpy.ndarray[object[3, 1]]) -> numpy.ndarray[object[3, 1]]

Multiplies this RigidTransform X_AB by the position vector p_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.
1. multiply(self: pydrake.math.RigidTransform_[AutoDiffXd], p_BoQ_B: numpy.ndarray[object[3, n]]) -> numpy.ndarray[object[3, n]]

Multiplies this RigidTransform X_AB by the n position vectors p_BoQ1_Bp_BoQn_B, where p_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 vectors p_BoQi_B or an expression that resolves to a 3 x n matrix of position vectors.
Returns p_AoQ_A:
3 x n matrix with n position vectors p_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 that X_AB * p_BoQ_B returns p_AoQ_A = p_AoBo_A + R_AB * p_BoQ_B, where p_AoBo_A is the position vector from Ao to Bo expressed in A and R_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.
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.
set_rotation(*args, **kwargs)

1. 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).
1. 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:
“roll-pitch-yaw” angles.

RotationMatrix::RotationMatrix(const RollPitchYaw<T>&) which describes the parameter, preconditions, etc.

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

RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&) which describes the parameter, preconditions, exception conditions, etc.

1. 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 vector lambda.

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.
translation(self: pydrake.math.RigidTransform_[AutoDiffXd]) → numpy.ndarray[object[3, 1]]

Returns p_AoBo_A, the position vector portion of this 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 distance-preserving 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 right-handed orthogonal unit vectors Ax, Ay, Az fixed in frame A to right-handed 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 is R_AB. The monogram notation for the position vector from Ao to Bo is p_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 not X_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 (non-NAN) position vector. Eigen::Isometry does not. - RigidTransform catches bugs that are undetected by Eigen::Isometry. - RigidTransform has additional functionality and ease-of-use, 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 counter-intuitive 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).
Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.
static FromMatrix4(matrix: numpy.ndarray[object[4, 4]]) → pydrake.math.RigidTransform_[Expression]

Please use RigidTransfrom(pose=...) constructor. This will be removed on or around 2019-11-15.

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.
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 vector p_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().

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.

__init__(*args, **kwargs)

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

1. __init__(self: pydrake.math.RigidTransform_[Expression], other: pydrake.math.RigidTransform_[Expression]) -> None
2. __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.
1. __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 Space-fixed (extrinsic) X-Y-Z rotation with “roll-pitch-yaw” angles [r, p, y] or equivalently a Body- fixed (intrinsic) Z-Y-X rotation with “yaw-pitch-roll” angles [y, p, r].

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.
1. __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 non-zero, 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.

RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&)

1. __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 non-zero 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.

RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&)

1. __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).
1. __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.
1. __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 vector p_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().

1. __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);
1. __init__(self: pydrake.math.RigidTransform_[Expression], matrix: numpy.ndarray[object[4, 4]]) -> None

Please use RigidTransfrom(pose=...) constructor. This will be removed on or around 2019-11-15.

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 new RigidTransform<To> from a RigidTransform<From> but only if type To is constructible from type From. 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 vice-versa.

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 new RigidTransform<To> from a RigidTransform<From> but only if type To is constructible from type From. 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 vice-versa.

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 square-root 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 2019-12-01. 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 2019-12-01. See drake issue #9865 for details.

multiply(*args, **kwargs)

1. multiply(self: pydrake.math.RigidTransform_[Expression], other: pydrake.math.RigidTransform_[Expression]) -> pydrake.math.RigidTransform_[Expression]

Multiplies this RigidTransform X_AB by the other RigidTransform X_BC and returns the RigidTransform X_AC = X_AB * X_BC.

1. multiply(self: pydrake.math.RigidTransform_[Expression], p_BoQ_B: numpy.ndarray[object[3, 1]]) -> numpy.ndarray[object[3, 1]]

Multiplies this RigidTransform X_AB by the position vector p_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.
1. multiply(self: pydrake.math.RigidTransform_[Expression], p_BoQ_B: numpy.ndarray[object[3, n]]) -> numpy.ndarray[object[3, n]]

Multiplies this RigidTransform X_AB by the n position vectors p_BoQ1_Bp_BoQn_B, where p_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 vectors p_BoQi_B or an expression that resolves to a 3 x n matrix of position vectors.
Returns p_AoQ_A:
3 x n matrix with n position vectors p_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 that X_AB * p_BoQ_B returns p_AoQ_A = p_AoBo_A + R_AB * p_BoQ_B, where p_AoBo_A is the position vector from Ao to Bo expressed in A and R_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.
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.
set_rotation(*args, **kwargs)

1. 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).
1. 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:
“roll-pitch-yaw” angles.

RotationMatrix::RotationMatrix(const RollPitchYaw<T>&) which describes the parameter, preconditions, etc.

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

RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&) which describes the parameter, preconditions, exception conditions, etc.

1. 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 vector lambda.

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.
translation(self: pydrake.math.RigidTransform_[Expression]) → numpy.ndarray[object[3, 1]]

Returns p_AoBo_A, the position vector portion of this 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 distance-preserving 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 right-handed orthogonal unit vectors Ax, Ay, Az fixed in frame A to right-handed 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 is R_AB. The monogram notation for the position vector from Ao to Bo is p_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 not X_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 (non-NAN) position vector. Eigen::Isometry does not. - RigidTransform catches bugs that are undetected by Eigen::Isometry. - RigidTransform has additional functionality and ease-of-use, 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 counter-intuitive 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).
Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.
static FromMatrix4(matrix: numpy.ndarray[float64[4, 4]]) → pydrake.math.RigidTransform_[float]

Please use RigidTransfrom(pose=...) constructor. This will be removed on or around 2019-11-15.

GetAsIsometry3(self: pydrake.math.RigidTransform_[float]) → pydrake.common.eigen_geometry.Isometry3_[float]

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.
SetFromIsometry3(self: pydrake.math.RigidTransform_[float], pose: pydrake.common.eigen_geometry.Isometry3_[float]) → 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 vector p_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().

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.

__init__(*args, **kwargs)

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

1. __init__(self: pydrake.math.RigidTransform_[float], other: pydrake.math.RigidTransform_[float]) -> None
2. __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.
1. __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 Space-fixed (extrinsic) X-Y-Z rotation with “roll-pitch-yaw” angles [r, p, y] or equivalently a Body- fixed (intrinsic) Z-Y-X rotation with “yaw-pitch-roll” angles [y, p, r].

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.
1. __init__(self: pydrake.math.RigidTransform_[float], quaternion: pydrake.common.eigen_geometry.Quaternion_[float], p: numpy.ndarray[float64[3, 1]]) -> None

Constructs a RigidTransform from a Quaternion and a position vector.

Parameter quaternion:
a non-zero, 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.

RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&)

1. __init__(self: pydrake.math.RigidTransform_[float], theta_lambda: pydrake.common.eigen_geometry.AngleAxis_[float], 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 non-zero 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.

RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&)

1. __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).
1. __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.
1. __init__(self: pydrake.math.RigidTransform_[float], pose: pydrake.common.eigen_geometry.Isometry3_[float]) -> 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 vector p_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().

1. __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);
1. __init__(self: pydrake.math.RigidTransform_[float], matrix: numpy.ndarray[float64[4, 4]]) -> None

Please use RigidTransfrom(pose=...) constructor. This will be removed on or around 2019-11-15.

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 new RigidTransform<To> from a RigidTransform<From> but only if type To is constructible from type From. 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 vice-versa.

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 new RigidTransform<To> from a RigidTransform<From> but only if type To is constructible from type From. 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 vice-versa.

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 new RigidTransform<To> from a RigidTransform<From> but only if type To is constructible from type From. 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 vice-versa.

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 new RigidTransform<To> from a RigidTransform<From> but only if type To is constructible from type From. 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 vice-versa.

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 square-root 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 2019-12-01. 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 2019-12-01. See drake issue #9865 for details.

multiply(*args, **kwargs)

1. multiply(self: pydrake.math.RigidTransform_[float], other: pydrake.math.RigidTransform_[float]) -> pydrake.math.RigidTransform_[float]

Multiplies this RigidTransform X_AB by the other RigidTransform X_BC and returns the RigidTransform X_AC = X_AB * X_BC.

1. multiply(self: pydrake.math.RigidTransform_[float], p_BoQ_B: numpy.ndarray[float64[3, 1]]) -> numpy.ndarray[float64[3, 1]]

Multiplies this RigidTransform X_AB by the position vector p_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.
1. multiply(self: pydrake.math.RigidTransform_[float], p_BoQ_B: numpy.ndarray[float64[3, n]]) -> numpy.ndarray[float64[3, n]]

Multiplies this RigidTransform X_AB by the n position vectors p_BoQ1_Bp_BoQn_B, where p_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 vectors p_BoQi_B or an expression that resolves to a 3 x n matrix of position vectors.
Returns p_AoQ_A:
3 x n matrix with n position vectors p_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 that X_AB * p_BoQ_B returns p_AoQ_A = p_AoBo_A + R_AB * p_BoQ_B, where p_AoBo_A is the position vector from Ao to Bo expressed in A and R_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.
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.
set_rotation(*args, **kwargs)

1. 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).
1. 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:
“roll-pitch-yaw” angles.

RotationMatrix::RotationMatrix(const RollPitchYaw<T>&) which describes the parameter, preconditions, etc.

1. set_rotation(self: pydrake.math.RigidTransform_[float], quaternion: pydrake.common.eigen_geometry.Quaternion_[float]) -> None

Sets the rotation part of this RigidTransform from a Quaternion.

Parameter quaternion:
a quaternion which may or may not have unit length.

RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&) which describes the parameter, preconditions, exception conditions, etc.

1. set_rotation(self: pydrake.math.RigidTransform_[float], theta_lambda: pydrake.common.eigen_geometry.AngleAxis_[float]) -> None

Sets the rotation part of this RigidTransform from an AngleAxis.

Parameter theta_lambda:
an angle theta (in radians) and vector lambda.

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.
translation(self: pydrake.math.RigidTransform_[float]) → numpy.ndarray[float64[3, 1]]

Returns p_AoBo_A, the position vector portion of this RigidTransform, i.e., position vector from Ao (frame A’s origin) to Bo (frame B’s origin).

pydrake.math.RollPitchYaw
template pydrake.math.RollPitchYaw_
class RollPitchYaw_[float]

This class represents the orientation between two arbitrary frames A and D associated with a Space-fixed (extrinsic) X-Y-Z rotation by “roll-pitch-yaw” angles [r, p, y], which is equivalent to a Body-fixed (intrinsic) Z-Y-X rotation by “yaw-pitch-roll” angles [y, p, r]. The rotation matrix R_AD associated with this roll-pitch-yaw [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 right-handed rotations relative to A.

• 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a

roll angle r about Dx = 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 about Cy = 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 about Bz = Az. Note: B and A are no longer aligned. The monogram notation for the rotation matrix relating A to D is R_AD.

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.

Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.

Instantiated templates for the following kinds of T’s are provided:

• double
• AutoDiffXd
• symbolic::Expression
CalcAngularVelocityInChildFromRpyDt(self: pydrake.math.RollPitchYaw_[float], rpyDt: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 1]]

Calculates angular velocity from this RollPitchYaw whose roll-pitch-yaw angles [r; p; y] relate the orientation of two generic frames A and D.

Parameter rpyDt:
Time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
Returns: w_AD_D, frame D’s angular velocity in frame A, expressed in D.
CalcAngularVelocityInParentFromRpyDt(self: pydrake.math.RollPitchYaw_[float], rpyDt: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 1]]

Calculates angular velocity from this RollPitchYaw whose roll-pitch-yaw angles [r; p; y] relate the orientation of two generic frames A and D.

Parameter rpyDt:
Time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
Returns: w_AD_A, frame D’s angular velocity in frame A, expressed in A.
CalcRotationMatrixDt(self: pydrake.math.RollPitchYaw_[float], rpyDt: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 3]]

Forms Ṙ, the ordinary derivative of the RotationMatrix R with respect to an independent variable t (t usually denotes time) and R is the RotationMatrix formed by this RollPitchYaw. The roll-pitch-yaw angles r, p, y are regarded as functions of t [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: Ṙ, the ordinary derivative of R with respect to t, calculated as Ṙ = ∂R/∂r * ṙ + ∂R/∂p * ṗ + ∂R/∂y * ẏ. In other words, the returned (i, j) element is ∂Rij/∂r * ṙ + ∂Rij/∂p * ṗ + ∂Rij/∂y * ẏ.
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ⁿᵈ time-derivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D.

Parameter rpyDt:
time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
, frame D’s angular acceleration in frame A, expressed in frame D.
Returns: [r̈, p̈, ÿ], the 2ⁿᵈ time-derivative of this RollPitchYaw. RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock).

Note

This method has a divide-by-zero 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., when cos(p) 0.

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ⁿᵈ time-derivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D.

Parameter rpyDt:
time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
, frame D’s angular acceleration in frame A, expressed in frame A.
Returns: [r̈, p̈, ÿ], the 2ⁿᵈ time-derivative of this RollPitchYaw. RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock).

Note

This method has a divide-by-zero 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., when cos(p) 0.

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ˢᵗ time-derivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D.

, frame D’s angular velocity in frame A, expressed in A.
Returns: [ṙ; ṗ; ẏ], the 1ˢᵗ time-derivative of this RollPitchYaw. RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock).

Note

This method has a divide-by-zero 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., when cos(p) 0.

ToQuaternion(self: pydrake.math.RollPitchYaw_[float]) → pydrake.common.eigen_geometry.Quaternion_[float]

Returns a quaternion representation of this RollPitchYaw.

ToRotationMatrix(self: pydrake.math.RollPitchYaw_[float]) → pydrake.math.RotationMatrix_[float]

Returns the RotationMatrix representation of this RollPitchYaw.

__init__(*args, **kwargs)

1. __init__(self: pydrake.math.RollPitchYaw_[float], other: pydrake.math.RollPitchYaw_[float]) -> None
2. __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).
1. __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:
x-directed angle in SpaceXYZ rotation sequence.
Parameter pitch:
y-directed angle in SpaceXYZ rotation sequence.
Parameter yaw:
z-directed angle in SpaceXYZ rotation sequence.
Raises: RuntimeError in debug builds if !IsValid(Vector3(roll, pitch, yaw)).
1. __init__(self: pydrake.math.RollPitchYaw_[float], R: pydrake.math.RotationMatrix_[float]) -> None

Uses a RotationMatrix to construct a RollPitchYaw with roll-pitch-yaw angles [r, p, y] in the range <= r <= π, -π/2 <= p <= π/2, -π <= y <= π.

Parameter R:
a RotationMatrix.

Note

This new high-accuracy algorithm avoids numerical round-off issues encountered by some algorithms when pitch is within 1E-6 of π/2 or -π/2.

1. __init__(self: pydrake.math.RollPitchYaw_[float], quaternion: pydrake.common.eigen_geometry.Quaternion_[float]) -> None

Uses a Quaternion to construct a RollPitchYaw with roll-pitch-yaw angles [r, p, y] in the range <= r <= π, -π/2 <= p <= π/2, -π <= y <= π.

Parameter quaternion:
unit Quaternion.

Note

This new high-accuracy algorithm avoids numerical round-off issues encountered by some algorithms when pitch is within 1E-6 of π/2 or -π/2.

Raises: RuntimeError in debug builds if !IsValid(rpy).
1. __init__(self: pydrake.math.RollPitchYaw_[float], matrix: numpy.ndarray[float64[3, 3]]) -> None

Construct from raw rotation matrix. See RotationMatrix overload for more information.

pitch_angle(self: pydrake.math.RollPitchYaw_[float]) → float

Returns the pitch-angle underlying this RollPitchYaw.

roll_angle(self: pydrake.math.RollPitchYaw_[float]) → float

Returns the roll-angle underlying 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 yaw-angle underlying this RollPitchYaw.

class pydrake.math.RollPitchYaw_[AutoDiffXd]

This class represents the orientation between two arbitrary frames A and D associated with a Space-fixed (extrinsic) X-Y-Z rotation by “roll-pitch-yaw” angles [r, p, y], which is equivalent to a Body-fixed (intrinsic) Z-Y-X rotation by “yaw-pitch-roll” angles [y, p, r]. The rotation matrix R_AD associated with this roll-pitch-yaw [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 right-handed rotations relative to A.

• 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a

roll angle r about Dx = 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 about Cy = 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 about Bz = Az. Note: B and A are no longer aligned. The monogram notation for the rotation matrix relating A to D is R_AD.

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.

Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.

Instantiated templates for the following kinds of T’s are provided:

• double
• AutoDiffXd
• symbolic::Expression
CalcAngularVelocityInChildFromRpyDt(self: pydrake.math.RollPitchYaw_[AutoDiffXd], rpyDt: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 1]]

Calculates angular velocity from this RollPitchYaw whose roll-pitch-yaw angles [r; p; y] relate the orientation of two generic frames A and D.

Parameter rpyDt:
Time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
Returns: w_AD_D, frame D’s angular velocity in frame A, expressed in D.
CalcAngularVelocityInParentFromRpyDt(self: pydrake.math.RollPitchYaw_[AutoDiffXd], rpyDt: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 1]]

Calculates angular velocity from this RollPitchYaw whose roll-pitch-yaw angles [r; p; y] relate the orientation of two generic frames A and D.

Parameter rpyDt:
Time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
Returns: w_AD_A, frame D’s angular velocity in frame A, expressed in A.
CalcRotationMatrixDt(self: pydrake.math.RollPitchYaw_[AutoDiffXd], rpyDt: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 3]]

Forms Ṙ, the ordinary derivative of the RotationMatrix R with respect to an independent variable t (t usually denotes time) and R is the RotationMatrix formed by this RollPitchYaw. The roll-pitch-yaw angles r, p, y are regarded as functions of t [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: Ṙ, the ordinary derivative of R with respect to t, calculated as Ṙ = ∂R/∂r * ṙ + ∂R/∂p * ṗ + ∂R/∂y * ẏ. In other words, the returned (i, j) element is ∂Rij/∂r * ṙ + ∂Rij/∂p * ṗ + ∂Rij/∂y * ẏ.
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ⁿᵈ time-derivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D.

Parameter rpyDt:
time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
, frame D’s angular acceleration in frame A, expressed in frame D.
Returns: [r̈, p̈, ÿ], the 2ⁿᵈ time-derivative of this RollPitchYaw. RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock).

Note

This method has a divide-by-zero 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., when cos(p) 0.

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ⁿᵈ time-derivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D.

Parameter rpyDt:
time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
, frame D’s angular acceleration in frame A, expressed in frame A.
Returns: [r̈, p̈, ÿ], the 2ⁿᵈ time-derivative of this RollPitchYaw. RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock).

Note

This method has a divide-by-zero 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., when cos(p) 0.

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ˢᵗ time-derivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D.

, frame D’s angular velocity in frame A, expressed in A.
Returns: [ṙ; ṗ; ẏ], the 1ˢᵗ time-derivative of this RollPitchYaw. RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock).

Note

This method has a divide-by-zero 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., when cos(p) 0.

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.

__init__(*args, **kwargs)

1. __init__(self: pydrake.math.RollPitchYaw_[AutoDiffXd], other: pydrake.math.RollPitchYaw_[AutoDiffXd]) -> None
2. __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).
1. __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:
x-directed angle in SpaceXYZ rotation sequence.
Parameter pitch:
y-directed angle in SpaceXYZ rotation sequence.
Parameter yaw:
z-directed angle in SpaceXYZ rotation sequence.
Raises: RuntimeError in debug builds if !IsValid(Vector3(roll, pitch, yaw)).
1. __init__(self: pydrake.math.RollPitchYaw_[AutoDiffXd], R: pydrake.math.RotationMatrix_[AutoDiffXd]) -> None

Uses a RotationMatrix to construct a RollPitchYaw with roll-pitch-yaw angles [r, p, y] in the range <= r <= π, -π/2 <= p <= π/2, -π <= y <= π.

Parameter R:
a RotationMatrix.

Note

This new high-accuracy algorithm avoids numerical round-off issues encountered by some algorithms when pitch is within 1E-6 of π/2 or -π/2.

1. __init__(self: pydrake.math.RollPitchYaw_[AutoDiffXd], quaternion: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd]) -> None

Uses a Quaternion to construct a RollPitchYaw with roll-pitch-yaw angles [r, p, y] in the range <= r <= π, -π/2 <= p <= π/2, -π <= y <= π.

Parameter quaternion:
unit Quaternion.

Note

This new high-accuracy algorithm avoids numerical round-off issues encountered by some algorithms when pitch is within 1E-6 of π/2 or -π/2.

Raises: RuntimeError in debug builds if !IsValid(rpy).
1. __init__(self: pydrake.math.RollPitchYaw_[AutoDiffXd], matrix: numpy.ndarray[object[3, 3]]) -> None

Construct from raw rotation matrix. See RotationMatrix overload for more information.

pitch_angle(self: pydrake.math.RollPitchYaw_[AutoDiffXd]) → pydrake.autodiffutils.AutoDiffXd

Returns the pitch-angle underlying this RollPitchYaw.

roll_angle(self: pydrake.math.RollPitchYaw_[AutoDiffXd]) → pydrake.autodiffutils.AutoDiffXd

Returns the roll-angle underlying 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 yaw-angle underlying this RollPitchYaw.

class pydrake.math.RollPitchYaw_[Expression]

This class represents the orientation between two arbitrary frames A and D associated with a Space-fixed (extrinsic) X-Y-Z rotation by “roll-pitch-yaw” angles [r, p, y], which is equivalent to a Body-fixed (intrinsic) Z-Y-X rotation by “yaw-pitch-roll” angles [y, p, r]. The rotation matrix R_AD associated with this roll-pitch-yaw [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 right-handed rotations relative to A.

• 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a

roll angle r about Dx = 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 about Cy = 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 about Bz = Az. Note: B and A are no longer aligned. The monogram notation for the rotation matrix relating A to D is R_AD.

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.

Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.

Instantiated templates for the following kinds of T’s are provided:

• double
• AutoDiffXd
• symbolic::Expression
CalcAngularVelocityInChildFromRpyDt(self: pydrake.math.RollPitchYaw_[Expression], rpyDt: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 1]]

Calculates angular velocity from this RollPitchYaw whose roll-pitch-yaw angles [r; p; y] relate the orientation of two generic frames A and D.

Parameter rpyDt:
Time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
Returns: w_AD_D, frame D’s angular velocity in frame A, expressed in D.
CalcAngularVelocityInParentFromRpyDt(self: pydrake.math.RollPitchYaw_[Expression], rpyDt: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 1]]

Calculates angular velocity from this RollPitchYaw whose roll-pitch-yaw angles [r; p; y] relate the orientation of two generic frames A and D.

Parameter rpyDt:
Time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
Returns: w_AD_A, frame D’s angular velocity in frame A, expressed in A.
CalcRotationMatrixDt(self: pydrake.math.RollPitchYaw_[Expression], rpyDt: numpy.ndarray[object[3, 1]]) → numpy.ndarray[object[3, 3]]

Forms Ṙ, the ordinary derivative of the RotationMatrix R with respect to an independent variable t (t usually denotes time) and R is the RotationMatrix formed by this RollPitchYaw. The roll-pitch-yaw angles r, p, y are regarded as functions of t [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: Ṙ, the ordinary derivative of R with respect to t, calculated as Ṙ = ∂R/∂r * ṙ + ∂R/∂p * ṗ + ∂R/∂y * ẏ. In other words, the returned (i, j) element is ∂Rij/∂r * ṙ + ∂Rij/∂p * ṗ + ∂Rij/∂y * ẏ.
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ⁿᵈ time-derivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D.

Parameter rpyDt:
time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
, frame D’s angular acceleration in frame A, expressed in frame D.
Returns: [r̈, p̈, ÿ], the 2ⁿᵈ time-derivative of this RollPitchYaw. RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock).

Note

This method has a divide-by-zero 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., when cos(p) 0.

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ⁿᵈ time-derivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D.

Parameter rpyDt:
time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
, frame D’s angular acceleration in frame A, expressed in frame A.
Returns: [r̈, p̈, ÿ], the 2ⁿᵈ time-derivative of this RollPitchYaw. RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock).

Note

This method has a divide-by-zero 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., when cos(p) 0.

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ˢᵗ time-derivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D.

, frame D’s angular velocity in frame A, expressed in A.
Returns: [ṙ; ṗ; ẏ], the 1ˢᵗ time-derivative of this RollPitchYaw. RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock).

Note

This method has a divide-by-zero 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., when cos(p) 0.

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.

__init__(*args, **kwargs)

1. __init__(self: pydrake.math.RollPitchYaw_[Expression], other: pydrake.math.RollPitchYaw_[Expression]) -> None
2. __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).
1. __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:
x-directed angle in SpaceXYZ rotation sequence.
Parameter pitch:
y-directed angle in SpaceXYZ rotation sequence.
Parameter yaw:
z-directed angle in SpaceXYZ rotation sequence.
Raises: RuntimeError in debug builds if !IsValid(Vector3(roll, pitch, yaw)).
1. __init__(self: pydrake.math.RollPitchYaw_[Expression], R: pydrake.math.RotationMatrix_[Expression]) -> None

Uses a RotationMatrix to construct a RollPitchYaw with roll-pitch-yaw angles [r, p, y] in the range <= r <= π, -π/2 <= p <= π/2, -π <= y <= π.

Parameter R:
a RotationMatrix.

Note

This new high-accuracy algorithm avoids numerical round-off issues encountered by some algorithms when pitch is within 1E-6 of π/2 or -π/2.

1. __init__(self: pydrake.math.RollPitchYaw_[Expression], quaternion: pydrake.common.eigen_geometry.Quaternion_[Expression]) -> None

Uses a Quaternion to construct a RollPitchYaw with roll-pitch-yaw angles [r, p, y] in the range <= r <= π, -π/2 <= p <= π/2, -π <= y <= π.

Parameter quaternion:
unit Quaternion.

Note

This new high-accuracy algorithm avoids numerical round-off issues encountered by some algorithms when pitch is within 1E-6 of π/2 or -π/2.

Raises: RuntimeError in debug builds if !IsValid(rpy).
1. __init__(self: pydrake.math.RollPitchYaw_[Expression], matrix: numpy.ndarray[object[3, 3]]) -> None

Construct from raw rotation matrix. See RotationMatrix overload for more information.

pitch_angle(self: pydrake.math.RollPitchYaw_[Expression]) → pydrake.symbolic.Expression

Returns the pitch-angle underlying this RollPitchYaw.

roll_angle(self: pydrake.math.RollPitchYaw_[Expression]) → pydrake.symbolic.Expression

Returns the roll-angle underlying 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 yaw-angle underlying this RollPitchYaw.

class pydrake.math.RollPitchYaw_[float]

This class represents the orientation between two arbitrary frames A and D associated with a Space-fixed (extrinsic) X-Y-Z rotation by “roll-pitch-yaw” angles [r, p, y], which is equivalent to a Body-fixed (intrinsic) Z-Y-X rotation by “yaw-pitch-roll” angles [y, p, r]. The rotation matrix R_AD associated with this roll-pitch-yaw [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 right-handed rotations relative to A.

• 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a

roll angle r about Dx = 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 about Cy = 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 about Bz = Az. Note: B and A are no longer aligned. The monogram notation for the rotation matrix relating A to D is R_AD.

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.

Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.

Instantiated templates for the following kinds of T’s are provided:

• double
• AutoDiffXd
• symbolic::Expression
CalcAngularVelocityInChildFromRpyDt(self: pydrake.math.RollPitchYaw_[float], rpyDt: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 1]]

Calculates angular velocity from this RollPitchYaw whose roll-pitch-yaw angles [r; p; y] relate the orientation of two generic frames A and D.

Parameter rpyDt:
Time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
Returns: w_AD_D, frame D’s angular velocity in frame A, expressed in D.
CalcAngularVelocityInParentFromRpyDt(self: pydrake.math.RollPitchYaw_[float], rpyDt: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 1]]

Calculates angular velocity from this RollPitchYaw whose roll-pitch-yaw angles [r; p; y] relate the orientation of two generic frames A and D.

Parameter rpyDt:
Time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
Returns: w_AD_A, frame D’s angular velocity in frame A, expressed in A.
CalcRotationMatrixDt(self: pydrake.math.RollPitchYaw_[float], rpyDt: numpy.ndarray[float64[3, 1]]) → numpy.ndarray[float64[3, 3]]

Forms Ṙ, the ordinary derivative of the RotationMatrix R with respect to an independent variable t (t usually denotes time) and R is the RotationMatrix formed by this RollPitchYaw. The roll-pitch-yaw angles r, p, y are regarded as functions of t [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: Ṙ, the ordinary derivative of R with respect to t, calculated as Ṙ = ∂R/∂r * ṙ + ∂R/∂p * ṗ + ∂R/∂y * ẏ. In other words, the returned (i, j) element is ∂Rij/∂r * ṙ + ∂Rij/∂p * ṗ + ∂Rij/∂y * ẏ.
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ⁿᵈ time-derivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D.

Parameter rpyDt:
time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
, frame D’s angular acceleration in frame A, expressed in frame D.
Returns: [r̈, p̈, ÿ], the 2ⁿᵈ time-derivative of this RollPitchYaw. RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock).

Note

This method has a divide-by-zero 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., when cos(p) 0.

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ⁿᵈ time-derivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D.

Parameter rpyDt:
time-derivative of [r; p; y], i.e., [ṙ; ṗ; ẏ].
, frame D’s angular acceleration in frame A, expressed in frame A.
Returns: [r̈, p̈, ÿ], the 2ⁿᵈ time-derivative of this RollPitchYaw. RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock).

Note

This method has a divide-by-zero 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., when cos(p) 0.

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ˢᵗ time-derivative of this RollPitchYaw whose angles [r; p; y] orient two generic frames A and D.

, frame D’s angular velocity in frame A, expressed in A.
Returns: [ṙ; ṗ; ẏ], the 1ˢᵗ time-derivative of this RollPitchYaw. RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock).

Note

This method has a divide-by-zero 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., when cos(p) 0.

ToQuaternion(self: pydrake.math.RollPitchYaw_[float]) → pydrake.common.eigen_geometry.Quaternion_[float]

Returns a quaternion representation of this RollPitchYaw.

ToRotationMatrix(self: pydrake.math.RollPitchYaw_[float]) → pydrake.math.RotationMatrix_[float]

Returns the RotationMatrix representation of this RollPitchYaw.

__init__(*args, **kwargs)

1. __init__(self: pydrake.math.RollPitchYaw_[float], other: pydrake.math.RollPitchYaw_[float]) -> None
2. __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).
1. __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:
x-directed angle in SpaceXYZ rotation sequence.
Parameter pitch:
y-directed angle in SpaceXYZ rotation sequence.
Parameter yaw:
z-directed angle in SpaceXYZ rotation sequence.
Raises: RuntimeError in debug builds if !IsValid(Vector3(roll, pitch, yaw)).
1. __init__(self: pydrake.math.RollPitchYaw_[float], R: pydrake.math.RotationMatrix_[float]) -> None

Uses a RotationMatrix to construct a RollPitchYaw with roll-pitch-yaw angles [r, p, y] in the range <= r <= π, -π/2 <= p <= π/2, -π <= y <= π.

Parameter R:
a RotationMatrix.

Note

This new high-accuracy algorithm avoids numerical round-off issues encountered by some algorithms when pitch is within 1E-6 of π/2 or -π/2.

1. __init__(self: pydrake.math.RollPitchYaw_[float], quaternion: pydrake.common.eigen_geometry.Quaternion_[float]) -> None

Uses a Quaternion to construct a RollPitchYaw with roll-pitch-yaw angles [r, p, y] in the range <= r <= π, -π/2 <= p <= π/2, -π <= y <= π.

Parameter quaternion:
unit Quaternion.

Note

This new high-accuracy algorithm avoids numerical round-off issues encountered by some algorithms when pitch is within 1E-6 of π/2 or -π/2.

Raises: RuntimeError in debug builds if !IsValid(rpy).
1. __init__(self: pydrake.math.RollPitchYaw_[float], matrix: numpy.ndarray[float64[3, 3]]) -> None

Construct from raw rotation matrix. See RotationMatrix overload for more information.

pitch_angle(self: pydrake.math.RollPitchYaw_[float]) → float

Returns the pitch-angle underlying this RollPitchYaw.

roll_angle(self: pydrake.math.RollPitchYaw_[float]) → float

Returns the roll-angle underlying 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 yaw-angle underlying this RollPitchYaw.

pydrake.math.RotationMatrix
template pydrake.math.RotationMatrix_
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 right-handed orthogonal unit vectors Ax, Ay, Az fixed in frame A to right-handed 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 is v_A = R_AB * v_B, where v_B denotes an arbitrary vector v expressed in terms of Bx, By, Bz and v_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).
Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.

Instantiated templates for the following kinds of T’s are provided:

• double
• AutoDiffXd
• symbolic::Expression
static Identity() → pydrake.math.RotationMatrix_[float]
IsExactlyIdentity(self: pydrake.math.RotationMatrix_[float]) → bool

Returns True if this 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 if this 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 angle theta about unit vector Ax = 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 right-handed rotation relative to A by an angle theta about Ax = Bx.

⎡ 1       0                 0  ⎤
R_AB = ⎢ 0   cos(theta)   -sin(theta) ⎥
⎣ 0   sin(theta)    cos(theta) ⎦
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 angle theta about unit vector Ay = 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 right-handed rotation relative to A by an angle theta about Ay = By.

⎡  cos(theta)   0   sin(theta) ⎤
R_AB = ⎢          0    1           0  ⎥
⎣ -sin(theta)   0   cos(theta) ⎦
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 angle theta about unit vector Az = 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 right-handed rotation relative to A by an angle theta about Az = Bz.

⎡ cos(theta)  -sin(theta)   0 ⎤
R_AB = ⎢ sin(theta)   cos(theta)   0 ⎥
⎣         0            0    1 ⎦
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 matrix-2 norm (or equivalently with a Frobenius norm). Hence, this method creates a RotationMatrix R from a 3x3 matrix M by minimizing ‖R - M‖₂ (the matrix-2 norm of (R-M)) subject to R * 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 double-summation ∑ᵢ ∑ⱼ (R(i,j) - M(i,j))² where i = 1:3, j = 1:3, subject to R * Rᵀ = I. The square-root of this double-summation 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. 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 matrix-2 norm (a.k.a an induced-2 norm), which is defined [Dahleh, Section 4.2] as the column matrix u which maximizes ‖(R - M) u‖ / ‖u‖, where u 0. Since the matrix-2 norm of any matrix A is equal to the maximum singular value of A, minimizing the matrix-2 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/electrical-engineering-and-computer-science/6-241j-dynamic-systems-and-control-spring-2011/readings/MIT6_241JS11_chap04.pdf

ToQuaternion(self: pydrake.math.RotationMatrix_[float]) → pydrake.common.eigen_geometry.Quaternion_[float]

Returns a quaternion q that represents this RotationMatrix. Since the quaternion q and -q represent the same RotationMatrix, this method chooses to return a canonical quaternion, i.e., with q(0) >= 0.

__init__(*args, **kwargs)

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

1. __init__(self: pydrake.math.RotationMatrix_[float], other: pydrake.math.RotationMatrix_[float]) -> None
2. __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).
1. __init__(self: pydrake.math.RotationMatrix_[float], quaternion: pydrake.common.eigen_geometry.Quaternion_[float]) -> None

Constructs a RotationMatrix from an Eigen::Quaternion.

Parameter quaternion:
a non-zero, 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 square-root associated with normalization.

1. __init__(self: pydrake.math.RotationMatrix_[float], theta_lambda: pydrake.common.eigen_geometry.AngleAxis_[float]) -> None

Constructs a RotationMatrix from an Eigen::AngleAxis.

Parameter theta_lambda:
an Eigen::AngleAxis whose associated axis (vector direction herein called lambda) is non-zero 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.
1. __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 Space-fixed (extrinsic) X-Y-Z rotation by “roll-pitch-yaw” angles [r, p, y], which is equivalent to a Body-fixed (intrinsic) Z-Y-X rotation by “yaw-pitch-roll” angles [y, p, r].

Parameter rpy:
radian measures of three angles [roll, pitch, yaw].
Parameter rpy:
a RollPitchYaw which is a Space-fixed (extrinsic) X-Y-Z rotation with “roll-pitch-yaw” angles [r, p, y] or equivalently a Body- fixed (intrinsic) Z-Y-X rotation with “yaw-pitch-roll” angles [y, p, r].

Note

Denoting roll r, pitch p, yaw y, this method returns a rotation matrix R_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 right-handed rotations relative to A.

• 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a

roll angle r about Dx = 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 about Cy = 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 about Bz = Az. Note: B and A are no longer aligned.

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 new RotationMatrix<To> from a RotationMatrix<From> but only if type To is constructible from type From. 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 vice-versa.

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 new RotationMatrix<To> from a RotationMatrix<From> but only if type To is constructible from type From. 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 vice-versa.

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 new RotationMatrix<To> from a RotationMatrix<From> but only if type To is constructible from type From. 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 vice-versa.

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 new RotationMatrix<To> from a RotationMatrix<From> but only if type To is constructible from type From. 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 vice-versa.

col(self: pydrake.math.RotationMatrix_[float], index: int) → numpy.ndarray[float64[3, 1]]

Returns this rotation matrix’s iᵗʰ column (i = 0, 1, 2). For this rotation matrix R_AB (which relates right-handed 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).

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); or Vector3<T> Bz_A = col(2);

inverse(self: pydrake.math.RotationMatrix_[float]) → pydrake.math.RotationMatrix_[float]
matrix(self: pydrake.math.RotationMatrix_[float]) → numpy.ndarray[float64[3, 3]]

Returns the Matrix3 underlying a RotationMatrix.

col(), row()

multiply(*args, **kwargs)

1. multiply(self: pydrake.math.RotationMatrix_[float], other: pydrake.math.RotationMatrix_[float]) -> pydrake.math.RotationMatrix_[float]

Calculates this rotation matrix R_AB multiplied by other rotation matrix R_BC, returning the composition R_AB * R_BC.

Parameter other:
RotationMatrix that post-multiplies this.
Returns: rotation matrix that results from this multiplied by other.

Note

It is possible (albeit improbable) to create an invalid rotation matrix by accumulating round-off error with a large number of multiplies.

1. multiply(self: pydrake.math.RotationMatrix_[float], v_B: numpy.ndarray[float64[3, 1]]) -> numpy.ndarray[float64[3, 1]]

Calculates this rotation matrix R_AB multiplied by an arbitrary Vector3 expressed in the B frame.

Parameter v_B:
3x1 vector that post-multiplies this.
Returns: 3x1 vector v_A = R_AB * v_B.
1. multiply(self: pydrake.math.RotationMatrix_[float], v_B: numpy.ndarray[float64[3, n]]) -> numpy.ndarray[float64[3, n]]

Multiplies this RotationMatrix R_AB by the n vectors v1, … 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 vectors v1, … vn expressed in frame B.
Returns v_A:
3 x n matrix whose n columns are vectors v1, … 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;
row(self: pydrake.math.RotationMatrix_[float], index: int) → numpy.ndarray[float64[1, 3]]

Returns this rotation matrix’s iᵗʰ row (i = 0, 1, 2). For this rotation matrix R_AB (which relates right-handed 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).

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); or RowVector3<T> Az_B = row(2);

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).
transpose(self: pydrake.math.RotationMatrix_[float]) → pydrake.math.RotationMatrix_[float]
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 right-handed orthogonal unit vectors Ax, Ay, Az fixed in frame A to right-handed 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 is v_A = R_AB * v_B, where v_B denotes an arbitrary vector v expressed in terms of Bx, By, Bz and v_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).
Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.

Instantiated templates for the following kinds of T’s are provided:

• double
• AutoDiffXd
• symbolic::Expression
static Identity() → pydrake.math.RotationMatrix_[AutoDiffXd]
IsExactlyIdentity(self: pydrake.math.RotationMatrix_[AutoDiffXd]) → bool

Returns True if this 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 if this 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 angle theta about unit vector Ax = 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 right-handed rotation relative to A by an angle theta about Ax = Bx.

⎡ 1       0                 0  ⎤
R_AB = ⎢ 0   cos(theta)   -sin(theta) ⎥
⎣ 0   sin(theta)    cos(theta) ⎦
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 angle theta about unit vector Ay = 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 right-handed rotation relative to A by an angle theta about Ay = By.

⎡  cos(theta)   0   sin(theta) ⎤
R_AB = ⎢          0    1           0  ⎥
⎣ -sin(theta)   0   cos(theta) ⎦
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 angle theta about unit vector Az = 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 right-handed rotation relative to A by an angle theta about Az = Bz.

⎡ cos(theta)  -sin(theta)   0 ⎤
R_AB = ⎢ sin(theta)   cos(theta)   0 ⎥
⎣         0            0    1 ⎦
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 matrix-2 norm (or equivalently with a Frobenius norm). Hence, this method creates a RotationMatrix R from a 3x3 matrix M by minimizing ‖R - M‖₂ (the matrix-2 norm of (R-M)) subject to R * 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 double-summation ∑ᵢ ∑ⱼ (R(i,j) - M(i,j))² where i = 1:3, j = 1:3, subject to R * Rᵀ = I. The square-root of this double-summation 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. 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 matrix-2 norm (a.k.a an induced-2 norm), which is defined [Dahleh, Section 4.2] as the column matrix u which maximizes ‖(R - M) u‖ / ‖u‖, where u 0. Since the matrix-2 norm of any matrix A is equal to the maximum singular value of A, minimizing the matrix-2 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/electrical-engineering-and-computer-science/6-241j-dynamic-systems-and-control-spring-2011/readings/MIT6_241JS11_chap04.pdf

ToQuaternion(self: pydrake.math.RotationMatrix_[AutoDiffXd]) → pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd]

Returns a quaternion q that represents this RotationMatrix. Since the quaternion q and -q represent the same RotationMatrix, this method chooses to return a canonical quaternion, i.e., with q(0) >= 0.

__init__(*args, **kwargs)

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

1. __init__(self: pydrake.math.RotationMatrix_[AutoDiffXd], other: pydrake.math.RotationMatrix_[AutoDiffXd]) -> None
2. __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).
1. __init__(self: pydrake.math.RotationMatrix_[AutoDiffXd], quaternion: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd]) -> None

Constructs a RotationMatrix from an Eigen::Quaternion.

Parameter quaternion:
a non-zero, 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 square-root associated with normalization.

1. __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 non-zero 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.
1. __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 Space-fixed (extrinsic) X-Y-Z rotation by “roll-pitch-yaw” angles [r, p, y], which is equivalent to a Body-fixed (intrinsic) Z-Y-X rotation by “yaw-pitch-roll” angles [y, p, r].

Parameter rpy:
radian measures of three angles [roll, pitch, yaw].
Parameter rpy:
a RollPitchYaw which is a Space-fixed (extrinsic) X-Y-Z rotation with “roll-pitch-yaw” angles [r, p, y] or equivalently a Body- fixed (intrinsic) Z-Y-X rotation with “yaw-pitch-roll” angles [y, p, r].

Note

Denoting roll r, pitch p, yaw y, this method returns a rotation matrix R_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 right-handed rotations relative to A.

• 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a

roll angle r about Dx = 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 about Cy = 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 about Bz = Az. Note: B and A are no longer aligned.

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 new RotationMatrix<To> from a RotationMatrix<From> but only if type To is constructible from type From. 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 vice-versa.

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 new RotationMatrix<To> from a RotationMatrix<From> but only if type To is constructible from type From. 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 vice-versa.

col(self: pydrake.math.RotationMatrix_[AutoDiffXd], index: int) → numpy.ndarray[object[3, 1]]

Returns this rotation matrix’s iᵗʰ column (i = 0, 1, 2). For this rotation matrix R_AB (which relates right-handed 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).

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); or Vector3<T> Bz_A = col(2);

inverse(self: pydrake.math.RotationMatrix_[AutoDiffXd]) → pydrake.math.RotationMatrix_[AutoDiffXd]
matrix(self: pydrake.math.RotationMatrix_[AutoDiffXd]) → numpy.ndarray[object[3, 3]]

Returns the Matrix3 underlying a RotationMatrix.

col(), row()

multiply(*args, **kwargs)

1. multiply(self: pydrake.math.RotationMatrix_[AutoDiffXd], other: pydrake.math.RotationMatrix_[AutoDiffXd]) -> pydrake.math.RotationMatrix_[AutoDiffXd]

Calculates this rotation matrix R_AB multiplied by other rotation matrix R_BC, returning the composition R_AB * R_BC.

Parameter other:
RotationMatrix that post-multiplies this.
Returns: rotation matrix that results from this multiplied by other.

Note

It is possible (albeit improbable) to create an invalid rotation matrix by accumulating round-off error with a large number of multiplies.

1. multiply(self: pydrake.math.RotationMatrix_[AutoDiffXd], v_B: numpy.ndarray[object[3, 1]]) -> numpy.ndarray[object[3, 1]]

Calculates this rotation matrix R_AB multiplied by an arbitrary Vector3 expressed in the B frame.

Parameter v_B:
3x1 vector that post-multiplies this.
Returns: 3x1 vector v_A = R_AB * v_B.
1. multiply(self: pydrake.math.RotationMatrix_[AutoDiffXd], v_B: numpy.ndarray[object[3, n]]) -> numpy.ndarray[object[3, n]]

Multiplies this RotationMatrix R_AB by the n vectors v1, … 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 vectors v1, … vn expressed in frame B.
Returns v_A:
3 x n matrix whose n columns are vectors v1, … 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;
row(self: pydrake.math.RotationMatrix_[AutoDiffXd], index: int) → numpy.ndarray[object[1, 3]]

Returns this rotation matrix’s iᵗʰ row (i = 0, 1, 2). For this rotation matrix R_AB (which relates right-handed 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).

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); or RowVector3<T> Az_B = row(2);

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).
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 right-handed orthogonal unit vectors Ax, Ay, Az fixed in frame A to right-handed 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 is v_A = R_AB * v_B, where v_B denotes an arbitrary vector v expressed in terms of Bx, By, Bz and v_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).
Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.

Instantiated templates for the following kinds of T’s are provided:

• double
• AutoDiffXd
• symbolic::Expression
static Identity() → pydrake.math.RotationMatrix_[Expression]
IsExactlyIdentity(self: pydrake.math.RotationMatrix_[Expression]) → pydrake.symbolic.Formula

Returns True if this 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 if this 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 angle theta about unit vector Ax = 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 right-handed rotation relative to A by an angle theta about Ax = Bx.

⎡ 1       0                 0  ⎤
R_AB = ⎢ 0   cos(theta)   -sin(theta) ⎥
⎣ 0   sin(theta)    cos(theta) ⎦
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 angle theta about unit vector Ay = 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 right-handed rotation relative to A by an angle theta about Ay = By.

⎡  cos(theta)   0   sin(theta) ⎤
R_AB = ⎢          0    1           0  ⎥
⎣ -sin(theta)   0   cos(theta) ⎦
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 angle theta about unit vector Az = 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 right-handed rotation relative to A by an angle theta about Az = Bz.

⎡ cos(theta)  -sin(theta)   0 ⎤
R_AB = ⎢ sin(theta)   cos(theta)   0 ⎥
⎣         0            0    1 ⎦
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 matrix-2 norm (or equivalently with a Frobenius norm). Hence, this method creates a RotationMatrix R from a 3x3 matrix M by minimizing ‖R - M‖₂ (the matrix-2 norm of (R-M)) subject to R * 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 double-summation ∑ᵢ ∑ⱼ (R(i,j) - M(i,j))² where i = 1:3, j = 1:3, subject to R * Rᵀ = I. The square-root of this double-summation 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. 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 matrix-2 norm (a.k.a an induced-2 norm), which is defined [Dahleh, Section 4.2] as the column matrix u which maximizes ‖(R - M) u‖ / ‖u‖, where u 0. Since the matrix-2 norm of any matrix A is equal to the maximum singular value of A, minimizing the matrix-2 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/electrical-engineering-and-computer-science/6-241j-dynamic-systems-and-control-spring-2011/readings/MIT6_241JS11_chap04.pdf

ToQuaternion(self: pydrake.math.RotationMatrix_[Expression]) → pydrake.common.eigen_geometry.Quaternion_[Expression]

Returns a quaternion q that represents this RotationMatrix. Since the quaternion q and -q represent the same RotationMatrix, this method chooses to return a canonical quaternion, i.e., with q(0) >= 0.

__init__(*args, **kwargs)

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

1. __init__(self: pydrake.math.RotationMatrix_[Expression], other: pydrake.math.RotationMatrix_[Expression]) -> None
2. __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).
1. __init__(self: pydrake.math.RotationMatrix_[Expression], quaternion: pydrake.common.eigen_geometry.Quaternion_[Expression]) -> None

Constructs a RotationMatrix from an Eigen::Quaternion.

Parameter quaternion:
a non-zero, 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 square-root associated with normalization.

1. __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 non-zero 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.
1. __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 Space-fixed (extrinsic) X-Y-Z rotation by “roll-pitch-yaw” angles [r, p, y], which is equivalent to a Body-fixed (intrinsic) Z-Y-X rotation by “yaw-pitch-roll” angles [y, p, r].

Parameter rpy:
radian measures of three angles [roll, pitch, yaw].
Parameter rpy:
a RollPitchYaw which is a Space-fixed (extrinsic) X-Y-Z rotation with “roll-pitch-yaw” angles [r, p, y] or equivalently a Body- fixed (intrinsic) Z-Y-X rotation with “yaw-pitch-roll” angles [y, p, r].

Note

Denoting roll r, pitch p, yaw y, this method returns a rotation matrix R_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 right-handed rotations relative to A.

• 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a

roll angle r about Dx = 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 about Cy = 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 about Bz = Az. Note: B and A are no longer aligned.

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 new RotationMatrix<To> from a RotationMatrix<From> but only if type To is constructible from type From. 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 vice-versa.

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 new RotationMatrix<To> from a RotationMatrix<From> but only if type To is constructible from type From. 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 vice-versa.

col(self: pydrake.math.RotationMatrix_[Expression], index: int) → numpy.ndarray[object[3, 1]]

Returns this rotation matrix’s iᵗʰ column (i = 0, 1, 2). For this rotation matrix R_AB (which relates right-handed 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).

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); or Vector3<T> Bz_A = col(2);

inverse(self: pydrake.math.RotationMatrix_[Expression]) → pydrake.math.RotationMatrix_[Expression]
matrix(self: pydrake.math.RotationMatrix_[Expression]) → numpy.ndarray[object[3, 3]]

Returns the Matrix3 underlying a RotationMatrix.

col(), row()

multiply(*args, **kwargs)

1. multiply(self: pydrake.math.RotationMatrix_[Expression], other: pydrake.math.RotationMatrix_[Expression]) -> pydrake.math.RotationMatrix_[Expression]

Calculates this rotation matrix R_AB multiplied by other rotation matrix R_BC, returning the composition R_AB * R_BC.

Parameter other:
RotationMatrix that post-multiplies this.
Returns: rotation matrix that results from this multiplied by other.

Note

It is possible (albeit improbable) to create an invalid rotation matrix by accumulating round-off error with a large number of multiplies.

1. multiply(self: pydrake.math.RotationMatrix_[Expression], v_B: numpy.ndarray[object[3, 1]]) -> numpy.ndarray[object[3, 1]]

Calculates this rotation matrix R_AB multiplied by an arbitrary Vector3 expressed in the B frame.

Parameter v_B:
3x1 vector that post-multiplies this.
Returns: 3x1 vector v_A = R_AB * v_B.
1. multiply(self: pydrake.math.RotationMatrix_[Expression], v_B: numpy.ndarray[object[3, n]]) -> numpy.ndarray[object[3, n]]

Multiplies this RotationMatrix R_AB by the n vectors v1, … 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 vectors v1, … vn expressed in frame B.
Returns v_A:
3 x n matrix whose n columns are vectors v1, … 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;
row(self: pydrake.math.RotationMatrix_[Expression], index: int) → numpy.ndarray[object[1, 3]]

Returns this rotation matrix’s iᵗʰ row (i = 0, 1, 2). For this rotation matrix R_AB (which relates right-handed 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).

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); or RowVector3<T> Az_B = row(2);

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).
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 right-handed orthogonal unit vectors Ax, Ay, Az fixed in frame A to right-handed 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 is v_A = R_AB * v_B, where v_B denotes an arbitrary vector v expressed in terms of Bx, By, Bz and v_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).
Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.

Instantiated templates for the following kinds of T’s are provided:

• double
• AutoDiffXd
• symbolic::Expression
static Identity() → pydrake.math.RotationMatrix_[float]
IsExactlyIdentity(self: pydrake.math.RotationMatrix_[float]) → bool

Returns True if this 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 if this 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 angle theta about unit vector Ax = 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 right-handed rotation relative to A by an angle theta about Ax = Bx.

⎡ 1       0                 0  ⎤
R_AB = ⎢ 0   cos(theta)   -sin(theta) ⎥
⎣ 0   sin(theta)    cos(theta) ⎦
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 angle theta about unit vector Ay = 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 right-handed rotation relative to A by an angle theta about Ay = By.

⎡  cos(theta)   0   sin(theta) ⎤
R_AB = ⎢          0    1           0  ⎥
⎣ -sin(theta)   0   cos(theta) ⎦
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 angle theta about unit vector Az = 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 right-handed rotation relative to A by an angle theta about Az = Bz.

⎡ cos(theta)  -sin(theta)   0 ⎤
R_AB = ⎢ sin(theta)   cos(theta)   0 ⎥
⎣         0            0    1 ⎦
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 matrix-2 norm (or equivalently with a Frobenius norm). Hence, this method creates a RotationMatrix R from a 3x3 matrix M by minimizing ‖R - M‖₂ (the matrix-2 norm of (R-M)) subject to R * 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 double-summation ∑ᵢ ∑ⱼ (R(i,j) - M(i,j))² where i = 1:3, j = 1:3, subject to R * Rᵀ = I. The square-root of this double-summation 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. 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 matrix-2 norm (a.k.a an induced-2 norm), which is defined [Dahleh, Section 4.2] as the column matrix u which maximizes ‖(R - M) u‖ / ‖u‖, where u 0. Since the matrix-2 norm of any matrix A is equal to the maximum singular value of A, minimizing the matrix-2 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/electrical-engineering-and-computer-science/6-241j-dynamic-systems-and-control-spring-2011/readings/MIT6_241JS11_chap04.pdf

ToQuaternion(self: pydrake.math.RotationMatrix_[float]) → pydrake.common.eigen_geometry.Quaternion_[float]

Returns a quaternion q that represents this RotationMatrix. Since the quaternion q and -q represent the same RotationMatrix, this method chooses to return a canonical quaternion, i.e., with q(0) >= 0.

__init__(*args, **kwargs)

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

1. __init__(self: pydrake.math.RotationMatrix_[float], other: pydrake.math.RotationMatrix_[float]) -> None
2. __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).
1. __init__(self: pydrake.math.RotationMatrix_[float], quaternion: pydrake.common.eigen_geometry.Quaternion_[float]) -> None

Constructs a RotationMatrix from an Eigen::Quaternion.

Parameter quaternion:
a non-zero, 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 square-root associated with normalization.

1. __init__(self: pydrake.math.RotationMatrix_[float], theta_lambda: pydrake.common.eigen_geometry.AngleAxis_[float]) -> None

Constructs a RotationMatrix from an Eigen::AngleAxis.

Parameter theta_lambda:
an Eigen::AngleAxis whose associated axis (vector direction herein called lambda) is non-zero 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.
1. __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 Space-fixed (extrinsic) X-Y-Z rotation by “roll-pitch-yaw” angles [r, p, y], which is equivalent to a Body-fixed (intrinsic) Z-Y-X rotation by “yaw-pitch-roll” angles [y, p, r].

Parameter rpy:
radian measures of three angles [roll, pitch, yaw].
Parameter rpy:
a RollPitchYaw which is a Space-fixed (extrinsic) X-Y-Z rotation with “roll-pitch-yaw” angles [r, p, y] or equivalently a Body- fixed (intrinsic) Z-Y-X rotation with “yaw-pitch-roll” angles [y, p, r].

Note

Denoting roll r, pitch p, yaw y, this method returns a rotation matrix R_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 right-handed rotations relative to A.

• 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a

roll angle r about Dx = 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 about Cy = 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 about Bz = Az. Note: B and A are no longer aligned.

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 new RotationMatrix<To> from a RotationMatrix<From> but only if type To is constructible from type From. 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 vice-versa.

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 new RotationMatrix<To> from a RotationMatrix<From> but only if type To is constructible from type From. 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 vice-versa.

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 new RotationMatrix<To> from a RotationMatrix<From> but only if type To is constructible from type From. 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 vice-versa.

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 new RotationMatrix<To> from a RotationMatrix<From> but only if type To is constructible from type From. 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 vice-versa.

col(self: pydrake.math.RotationMatrix_[float], index: int) → numpy.ndarray[float64[3, 1]]

Returns this rotation matrix’s iᵗʰ column (i = 0, 1, 2). For this rotation matrix R_AB (which relates right-handed 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).

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); or Vector3<T> Bz_A = col(2);

inverse(self: pydrake.math.RotationMatrix_[float]) → pydrake.math.RotationMatrix_[float]
matrix(self: pydrake.math.RotationMatrix_[float]) → numpy.ndarray[float64[3, 3]]

Returns the Matrix3 underlying a RotationMatrix.

col(), row()

multiply(*args, **kwargs)

1. multiply(self: pydrake.math.RotationMatrix_[float], other: pydrake.math.RotationMatrix_[float]) -> pydrake.math.RotationMatrix_[float]

Calculates this rotation matrix R_AB multiplied by other rotation matrix R_BC, returning the composition R_AB * R_BC.

Parameter other:
RotationMatrix that post-multiplies this.
Returns: rotation matrix that results from this multiplied by other.

Note

It is possible (albeit improbable) to create an invalid rotation matrix by accumulating round-off error with a large number of multiplies.

1. multiply(self: pydrake.math.RotationMatrix_[float], v_B: numpy.ndarray[float64[3, 1]]) -> numpy.ndarray[float64[3, 1]]

Calculates this rotation matrix R_AB multiplied by an arbitrary Vector3 expressed in the B frame.

Parameter v_B:
3x1 vector that post-multiplies this.
Returns: 3x1 vector v_A = R_AB * v_B.
1. multiply(self: pydrake.math.RotationMatrix_[float], v_B: numpy.ndarray[float64[3, n]]) -> numpy.ndarray[float64[3, n]]

Multiplies this RotationMatrix R_AB by the n vectors v1, … 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 vectors v1, … vn expressed in frame B.
Returns v_A:
3 x n matrix whose n columns are vectors v1, … 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;
row(self: pydrake.math.RotationMatrix_[float], index: int) → numpy.ndarray[float64[1, 3]]

Returns this rotation matrix’s iᵗʰ row (i = 0, 1, 2). For this rotation matrix R_AB (which relates right-handed 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).

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); or RowVector3<T> Az_B = row(2);

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).
transpose(self: pydrake.math.RotationMatrix_[float]) → pydrake.math.RotationMatrix_[float]
pydrake.math.abs(*args, **kwargs)

1. abs(arg0: float) -> float
2. abs(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
3. abs(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.acos(*args, **kwargs)

1. acos(arg0: float) -> float
2. acos(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
3. acos(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.asin(*args, **kwargs)

1. asin(arg0: float) -> float
2. asin(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
3. asin(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.atan(*args, **kwargs)

1. atan(arg0: float) -> float
2. atan(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.atan2(*args, **kwargs)

1. atan2(y: float, x: float) -> float
2. atan2(arg0: pydrake.autodiffutils.AutoDiffXd, arg1: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
3. atan2(arg0: pydrake.symbolic.Expression, arg1: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.ceil(*args, **kwargs)

1. ceil(arg0: float) -> float
2. ceil(arg0: pydrake.autodiffutils.AutoDiffXd) -> float
3. ceil(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.cos(*args, **kwargs)

1. cos(arg0: float) -> float
2. cos(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
3. cos(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.cosh(*args, **kwargs)

1. cosh(arg0: float) -> float
2. cosh(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
3. cosh(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.exp(*args, **kwargs)

1. exp(arg0: float) -> float
2. exp(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
3. exp(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.floor(*args, **kwargs)

1. floor(arg0: float) -> float
2. floor(arg0: pydrake.autodiffutils.AutoDiffXd) -> float
3. floor(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.inv(*args, **kwargs)

1. inv(arg0: numpy.ndarray[float64[m, n]]) -> numpy.ndarray[float64[m, n]]
2. inv(arg0: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
3. inv(arg0: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
pydrake.math.log(*args, **kwargs)

1. log(arg0: float) -> float
2. log(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
3. log(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.max(*args, **kwargs)

1. max(arg0: float, arg1: float) -> float
2. max(arg0: pydrake.autodiffutils.AutoDiffXd, arg1: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
3. max(arg0: pydrake.symbolic.Expression, arg1: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.min(*args, **kwargs)

1. min(arg0: float, arg1: float) -> float
2. min(arg0: pydrake.autodiffutils.AutoDiffXd, arg1: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
3. min(arg0: pydrake.symbolic.Expression, arg1: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.pow(*args, **kwargs)

1. pow(arg0: float, arg1: float) -> float
2. pow(arg0: pydrake.autodiffutils.AutoDiffXd, arg1: float) -> pydrake.autodiffutils.AutoDiffXd
3. pow(arg0: pydrake.symbolic.Expression, arg1: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.sin(*args, **kwargs)

1. sin(arg0: float) -> float
2. sin(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
3. sin(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.sinh(*args, **kwargs)

1. sinh(arg0: float) -> float
2. sinh(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
3. sinh(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.sqrt(*args, **kwargs)

1. sqrt(arg0: float) -> float
2. sqrt(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
3. sqrt(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.tan(*args, **kwargs)

1. tan(arg0: float) -> float
2. tan(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
3. tan(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
pydrake.math.tanh(*args, **kwargs)