pydrake.math
Bindings for math
, including overloads for scalar types and basic SE(3)
representations.
Note that arrays of symbolic scalar types, such as Variable
and
Expression
, are exposed using ndarray[object]
, and as such logical
operations are constrained to return boolean values given NumPy’s
implementation; this is not desirable, as one should really get a Formula
object. As a workaround, this module provides the following vectorized
operators, following suit with the operator
builtin module:
lt
, le
, eq
, ne
, ge
, and gt
.
As an example:
>>> x = np.array([Variable("x0"), Variable("x1")])
>>> y = np.array([Variable("y0"), Variable("y1")])
>>> x >= y
# This should throw a RuntimeError
>>> ge(x, y)
array([<Formula "(x0 >= y0)">, <Formula "(x1 >= y1)">], dtype=object)
- pydrake.math.abs(*args, **kwargs)
Overloaded function.
abs(arg0: float) -> float
abs(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
abs(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.acos(*args, **kwargs)
Overloaded function.
acos(arg0: float) -> float
acos(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
acos(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.arccos(*args, **kwargs)
Overloaded function.
arccos(arg0: float) -> float
arccos(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
arccos(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.arcsin(*args, **kwargs)
Overloaded function.
arcsin(arg0: float) -> float
arcsin(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
arcsin(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.arctan(*args, **kwargs)
Overloaded function.
arctan(arg0: float) -> float
arctan(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
arctan(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.arctan2(*args, **kwargs)
Overloaded function.
arctan2(y: float, x: float) -> float
arctan2(y: pydrake.autodiffutils.AutoDiffXd, x: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
arctan2(y: pydrake.symbolic.Expression, x: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.AreQuaternionsEqualForOrientation(*args, **kwargs)
Overloaded function.
AreQuaternionsEqualForOrientation(quat1: pydrake.common.eigen_geometry.Quaternion, quat2: pydrake.common.eigen_geometry.Quaternion, tolerance: float) -> bool
This function tests whether two quaternions represent the same orientation. This function converts each quaternion to its canonical form and tests whether the absolute value of the difference in corresponding elements of these canonical quaternions is within tolerance.
- Parameter
quat1
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat is analogous to the rotation matrix R_AB.
- Parameter
quat2
: Quaternion with a description analogous to quat1.
- Parameter
tolerance
: Nonnegative real scalar defining the allowable difference in the orientation described by quat1 and quat2.
- Returns
True
if quat1 and quat2 represent the same orientation (to within tolerance), otherwiseFalse
.
AreQuaternionsEqualForOrientation(quat1: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd], quat2: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd], tolerance: pydrake.autodiffutils.AutoDiffXd) -> bool
This function tests whether two quaternions represent the same orientation. This function converts each quaternion to its canonical form and tests whether the absolute value of the difference in corresponding elements of these canonical quaternions is within tolerance.
- Parameter
quat1
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat is analogous to the rotation matrix R_AB.
- Parameter
quat2
: Quaternion with a description analogous to quat1.
- Parameter
tolerance
: Nonnegative real scalar defining the allowable difference in the orientation described by quat1 and quat2.
- Returns
True
if quat1 and quat2 represent the same orientation (to within tolerance), otherwiseFalse
.
AreQuaternionsEqualForOrientation(quat1: pydrake.common.eigen_geometry.Quaternion_[Expression], quat2: pydrake.common.eigen_geometry.Quaternion_[Expression], tolerance: pydrake.symbolic.Expression) -> pydrake.symbolic.Formula
This function tests whether two quaternions represent the same orientation. This function converts each quaternion to its canonical form and tests whether the absolute value of the difference in corresponding elements of these canonical quaternions is within tolerance.
- Parameter
quat1
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat is analogous to the rotation matrix R_AB.
- Parameter
quat2
: Quaternion with a description analogous to quat1.
- Parameter
tolerance
: Nonnegative real scalar defining the allowable difference in the orientation described by quat1 and quat2.
- Returns
True
if quat1 and quat2 represent the same orientation (to within tolerance), otherwiseFalse
.
- pydrake.math.asin(*args, **kwargs)
Overloaded function.
asin(arg0: float) -> float
asin(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
asin(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.atan(*args, **kwargs)
Overloaded function.
atan(arg0: float) -> float
atan(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
atan(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.atan2(*args, **kwargs)
Overloaded function.
atan2(y: float, x: float) -> float
atan2(y: pydrake.autodiffutils.AutoDiffXd, x: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
atan2(y: pydrake.symbolic.Expression, x: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.BalanceQuadraticForms(S: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], P: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) numpy.ndarray[numpy.float64[m, n]]
Given two quadratic forms, x’Sx > 0 and x’Px, (with P symmetric and full rank), finds a change of variables x = Ty, which simultaneously diagonalizes both forms (as inspired by “balanced truncation” in model-order reduction [1]). In this note, we use abs(M) to indicate the elementwise absolute value.
Adapting from [1], we observe that there is a family of coordinate systems that can simultaneously diagonalize T’ST and T’PT. Using D to denote a diagonal matrix, we call the result S-normal if T’ST = I and abs(T’PT) = D⁻², call it P-normal if T’ST = D² and abs(T’PT) = I, and call it “balanced” if T’ST = D and abs(T’PT) = D⁻¹. Note that if P > 0, then T’PT = D⁻¹.
We find x=Ty such that T’ST = D and abs(T’PT) = D⁻¹, where D is diagonal. The recipe is: - Factorize S = LL’, and choose R=L⁻¹. - Take svd(RPR’) = UΣV’, and note that U=V for positive definite matrices, and V is U up to a sign flip of the singular vectors for all symmetric matrices. - Choose T = R’U Σ^{-1/4}, where the matrix exponent can be taken elementwise because Σ is diagonal. This gives T’ST = Σ^{-1/2} (by using U’U=I), and abs(T’PT) = Σ^{1/2}. If P > 0, then T’PT = Σ^{1/2}.
Note that the numerical “balancing” can address the absolute scaling of the quadratic forms, but not the relative scaling. To understand this, consider the scalar case: we have two quadratic functions, sx² and px², with s>0, p>0. We’d like to choose x=Ty so that sT²y² and pT²y² are “balanced” (we’d like them both to be close to y²). We’ll choose T=p^{-1/4}s^{-1/4}, which gives sx² = sqrt(s/p)y², and px² = sqrt(p/s)y². For instance if s=1e8 and p=1e8, then t=1e-4 and st^2 = pt^2 = 1. But if s=10, p=1e7, then t=0.01, and st^2 = 1e-3, pt^2 = 1e3.
In the matrix case, the absolute scaling is important – it ensures that the two quadratic forms have the same matrix condition number and makes them as close as possible to 1. Besides absolute scaling, in the matrix case the balancing transform diagonalizes both quadratic forms.
[1] B. Moore, “Principal component analysis in linear systems: Controllability, observability, and model reduction,” IEEE Trans. Automat. Contr., vol. 26, no. 1, pp. 17–32, Feb. 1981.
- 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
- __init__(self: pydrake.math.BarycentricMesh, arg0: List[Set[float]]) None
Constructs the mesh.
- Eval(self: pydrake.math.BarycentricMesh, arg0: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], arg1: numpy.ndarray[numpy.float64[m, 1]]) numpy.ndarray[numpy.float64[m, 1]]
Returns the function evaluated at
input
.
- EvalBarycentricWeights(self: pydrake.math.BarycentricMesh, arg0: numpy.ndarray[numpy.float64[m, 1]]) Tuple[numpy.ndarray[numpy.int32[m, 1]], numpy.ndarray[numpy.float64[m, 1]]]
Writes the mesh indices used for interpolation to
mesh_indices
, and the interpolating coefficients toweights
. Inputs that are outside the bounding box of the input_grid are interpolated as though they were projected (elementwise) to the closest face of the defined mesh.- Parameter
input
: must be a vector of length get_num_inputs().
- Parameter
mesh_indices
: is a pointer to a vector of length get_num_interpolants().
- Parameter
weights
: is a vector of coefficients (which sum to 1) of length get_num_interpolants().
- Parameter
- get_all_mesh_points(self: pydrake.math.BarycentricMesh) numpy.ndarray[numpy.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[numpy.float64[m, 1]]
Returns the position of a mesh point in the input space referenced by its scalar index to
point
.- Parameter
index
: must be in [0, get_num_mesh_points).
- Parameter
- get_num_interpolants(self: pydrake.math.BarycentricMesh) int
- get_num_mesh_points(self: pydrake.math.BarycentricMesh) int
- MeshValuesFrom(self: pydrake.math.BarycentricMesh, arg0: Callable[[numpy.ndarray[numpy.float64[m, 1]]], numpy.ndarray[numpy.float64[m, 1]]]) numpy.ndarray[numpy.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.Click to expand C++ code...
MatrixXd mesh_values = bary.MeshValuesFrom( [](const auto& x) { return Vector1d(std::sin(x[0])); });
- class pydrake.math.BsplineBasis
Given a set of non-descending breakpoints t₀ ≤ t₁ ≤ ⋅⋅⋅ ≤ tₘ, a B-spline basis of order k is a set of n + 1 (where n = m - k) piecewise polynomials of degree k - 1 defined over those breakpoints. The elements of this set are called “B-splines”. The vector (t₀, t₁, …, tₘ)’ is referred to as the “knot vector” of the basis and its elements are referred to as “knots”.
At a breakpoint with multiplicity p (i.e. a breakpoint that appears p times in the knot vector), B-splines are guaranteed to have Cᵏ⁻ᵖ⁻¹ continuity.
A B-spline curve using a B-spline basis B, is a parametric curve mapping parameter values in [tₖ₋₁, tₙ₊₁] to a vector space V. For t ∈ [tₖ₋₁, tₙ₊₁] the value of the curve is given by the linear combination of n + 1 control points, pᵢ ∈ V, with the elements of B evaluated at t.
For more information on B-splines and their uses, see (for example) Patrikalakis et al. [1].
[1] https://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node15.html
Note
This class is templated; see
BsplineBasis_
for the list of instantiations.- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.math.BsplineBasis) -> None
__init__(self: pydrake.math.BsplineBasis, order: int, knots: List[float]) -> None
Constructs a B-spline basis with the specified
order
andknots
.- Precondition:
knots
is sorted in non-descending order.
- Raises
RuntimeError if knots.size() < 2 * order. –
__init__(self: pydrake.math.BsplineBasis, order: int, num_basis_functions: int, type: pydrake.math.KnotVectorType = <KnotVectorType.kClampedUniform: 1>, initial_parameter_value: float = 0.0, final_parameter_value: float = 1.0) -> None
Constructs a B-spline basis with the specified
order
, num_basis_functions,initial_parameter_value
, final_parameter_value, and an auto-generated knot vector of the specifiedtype
.- Raises
RuntimeError if num_basis_functions < order –
- Precondition:
initial_parameter_value ≤ final_parameter_value
__init__(self: pydrake.math.BsplineBasis, other: pydrake.math.BsplineBasis) -> None
- ComputeActiveBasisFunctionIndices(*args, **kwargs)
Overloaded function.
ComputeActiveBasisFunctionIndices(self: pydrake.math.BsplineBasis, parameter_interval: Annotated[List[float], FixedSize(2)]) -> List[int]
Returns the indices of the basis functions which may evaluate to non-zero values for some parameter value in
parameter_interval
; all other basis functions are strictly zero overparameter_interval
.- Precondition:
parameter_interval[0] ≤ parameter_interval[1]
- Precondition:
parameter_interval[0] ≥ initial_parameter_value()
- Precondition:
parameter_interval[1] ≤ final_parameter_value()
ComputeActiveBasisFunctionIndices(self: pydrake.math.BsplineBasis, parameter_value: float) -> List[int]
Returns the indices of the basis functions which may evaluate to non-zero values for
parameter_value
; all other basis functions are strictly zero at this point.- Precondition:
parameter_value ≥ initial_parameter_value()
- Precondition:
parameter_value ≤ final_parameter_value()
- degree(self: pydrake.math.BsplineBasis) int
The degree of the piecewise polynomials comprising this B-spline basis (k - 1 in the class description).
- EvaluateBasisFunctionI(self: pydrake.math.BsplineBasis, i: int, parameter_value: float) float
Returns the value of the
i
-th basis function evaluated atparameter_value
.
- EvaluateCurve(self: pydrake.math.BsplineBasis, control_points: List[numpy.ndarray[numpy.float64[m, 1]]], parameter_value: float) numpy.ndarray[numpy.float64[m, 1]]
Evaluates the B-spline curve defined by
this
andcontrol_points
at the givenparameter_value
.- Parameter
control_points
: Control points of the B-spline curve.
- Parameter
parameter_value
: Parameter value at which to evaluate the B-spline curve defined by
this
andcontrol_points
.- Precondition:
control_points.size() == num_basis_functions()
- Precondition:
parameter_value ≥ initial_parameter_value()
- Precondition:
parameter_value ≤ final_parameter_value()
- Parameter
- final_parameter_value(self: pydrake.math.BsplineBasis) float
The maximum allowable parameter value for B-spline curves using this basis (tₙ₊₁ in the class description).
- FindContainingInterval(self: pydrake.math.BsplineBasis, parameter_value: float) int
For a
parameter_value
= t, the interval that contains it is the pair of knot values [tᵢ, tᵢ₊₁] for the greatest i such that tᵢ ≤ t and tᵢ < final_parameter_value(). This function returns that value of i.- Precondition:
parameter_value ≥ initial_parameter_value()
- Precondition:
parameter_value ≤ final_parameter_value()
- initial_parameter_value(self: pydrake.math.BsplineBasis) float
The minimum allowable parameter value for B-spline curves using this basis (tₖ₋₁ in the class description).
- knots(self: pydrake.math.BsplineBasis) List[float]
The knot vector of this B-spline basis (the vector (t₀, t₁, …, tₘ)’ in the class description).
- num_basis_functions(self: pydrake.math.BsplineBasis) int
The number of basis functions in this B-spline basis (n + 1 in the class description).
- order(self: pydrake.math.BsplineBasis) int
The order of this B-spline basis (k in the class description).
- template pydrake.math.BsplineBasis_
Instantiations:
BsplineBasis_[float]
,BsplineBasis_[AutoDiffXd]
,BsplineBasis_[Expression]
- class pydrake.math.BsplineBasis_[AutoDiffXd]
Given a set of non-descending breakpoints t₀ ≤ t₁ ≤ ⋅⋅⋅ ≤ tₘ, a B-spline basis of order k is a set of n + 1 (where n = m - k) piecewise polynomials of degree k - 1 defined over those breakpoints. The elements of this set are called “B-splines”. The vector (t₀, t₁, …, tₘ)’ is referred to as the “knot vector” of the basis and its elements are referred to as “knots”.
At a breakpoint with multiplicity p (i.e. a breakpoint that appears p times in the knot vector), B-splines are guaranteed to have Cᵏ⁻ᵖ⁻¹ continuity.
A B-spline curve using a B-spline basis B, is a parametric curve mapping parameter values in [tₖ₋₁, tₙ₊₁] to a vector space V. For t ∈ [tₖ₋₁, tₙ₊₁] the value of the curve is given by the linear combination of n + 1 control points, pᵢ ∈ V, with the elements of B evaluated at t.
For more information on B-splines and their uses, see (for example) Patrikalakis et al. [1].
[1] https://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node15.html
- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.math.BsplineBasis_[AutoDiffXd]) -> None
__init__(self: pydrake.math.BsplineBasis_[AutoDiffXd], order: int, knots: List[pydrake.autodiffutils.AutoDiffXd]) -> None
Constructs a B-spline basis with the specified
order
andknots
.- Precondition:
knots
is sorted in non-descending order.
- Raises
RuntimeError if knots.size() < 2 * order. –
__init__(self: pydrake.math.BsplineBasis_[AutoDiffXd], order: int, num_basis_functions: int, type: pydrake.math.KnotVectorType = <KnotVectorType.kClampedUniform: 1>, initial_parameter_value: pydrake.autodiffutils.AutoDiffXd = 0.0, final_parameter_value: pydrake.autodiffutils.AutoDiffXd = 1.0) -> None
Constructs a B-spline basis with the specified
order
, num_basis_functions,initial_parameter_value
, final_parameter_value, and an auto-generated knot vector of the specifiedtype
.- Raises
RuntimeError if num_basis_functions < order –
- Precondition:
initial_parameter_value ≤ final_parameter_value
__init__(self: pydrake.math.BsplineBasis_[AutoDiffXd], other: pydrake.math.BsplineBasis_[AutoDiffXd]) -> None
- ComputeActiveBasisFunctionIndices(*args, **kwargs)
Overloaded function.
ComputeActiveBasisFunctionIndices(self: pydrake.math.BsplineBasis_[AutoDiffXd], parameter_interval: Annotated[List[pydrake.autodiffutils.AutoDiffXd], FixedSize(2)]) -> List[int]
Returns the indices of the basis functions which may evaluate to non-zero values for some parameter value in
parameter_interval
; all other basis functions are strictly zero overparameter_interval
.- Precondition:
parameter_interval[0] ≤ parameter_interval[1]
- Precondition:
parameter_interval[0] ≥ initial_parameter_value()
- Precondition:
parameter_interval[1] ≤ final_parameter_value()
ComputeActiveBasisFunctionIndices(self: pydrake.math.BsplineBasis_[AutoDiffXd], parameter_value: pydrake.autodiffutils.AutoDiffXd) -> List[int]
Returns the indices of the basis functions which may evaluate to non-zero values for
parameter_value
; all other basis functions are strictly zero at this point.- Precondition:
parameter_value ≥ initial_parameter_value()
- Precondition:
parameter_value ≤ final_parameter_value()
- degree(self: pydrake.math.BsplineBasis_[AutoDiffXd]) int
The degree of the piecewise polynomials comprising this B-spline basis (k - 1 in the class description).
- EvaluateBasisFunctionI(self: pydrake.math.BsplineBasis_[AutoDiffXd], i: int, parameter_value: pydrake.autodiffutils.AutoDiffXd) pydrake.autodiffutils.AutoDiffXd
Returns the value of the
i
-th basis function evaluated atparameter_value
.
- EvaluateCurve(self: pydrake.math.BsplineBasis_[AutoDiffXd], control_points: List[numpy.ndarray[object[m, 1]]], parameter_value: pydrake.autodiffutils.AutoDiffXd) numpy.ndarray[object[m, 1]]
Evaluates the B-spline curve defined by
this
andcontrol_points
at the givenparameter_value
.- Parameter
control_points
: Control points of the B-spline curve.
- Parameter
parameter_value
: Parameter value at which to evaluate the B-spline curve defined by
this
andcontrol_points
.- Precondition:
control_points.size() == num_basis_functions()
- Precondition:
parameter_value ≥ initial_parameter_value()
- Precondition:
parameter_value ≤ final_parameter_value()
- Parameter
- final_parameter_value(self: pydrake.math.BsplineBasis_[AutoDiffXd]) pydrake.autodiffutils.AutoDiffXd
The maximum allowable parameter value for B-spline curves using this basis (tₙ₊₁ in the class description).
- FindContainingInterval(self: pydrake.math.BsplineBasis_[AutoDiffXd], parameter_value: pydrake.autodiffutils.AutoDiffXd) int
For a
parameter_value
= t, the interval that contains it is the pair of knot values [tᵢ, tᵢ₊₁] for the greatest i such that tᵢ ≤ t and tᵢ < final_parameter_value(). This function returns that value of i.- Precondition:
parameter_value ≥ initial_parameter_value()
- Precondition:
parameter_value ≤ final_parameter_value()
- initial_parameter_value(self: pydrake.math.BsplineBasis_[AutoDiffXd]) pydrake.autodiffutils.AutoDiffXd
The minimum allowable parameter value for B-spline curves using this basis (tₖ₋₁ in the class description).
- knots(self: pydrake.math.BsplineBasis_[AutoDiffXd]) List[pydrake.autodiffutils.AutoDiffXd]
The knot vector of this B-spline basis (the vector (t₀, t₁, …, tₘ)’ in the class description).
- num_basis_functions(self: pydrake.math.BsplineBasis_[AutoDiffXd]) int
The number of basis functions in this B-spline basis (n + 1 in the class description).
- order(self: pydrake.math.BsplineBasis_[AutoDiffXd]) int
The order of this B-spline basis (k in the class description).
- class pydrake.math.BsplineBasis_[Expression]
Given a set of non-descending breakpoints t₀ ≤ t₁ ≤ ⋅⋅⋅ ≤ tₘ, a B-spline basis of order k is a set of n + 1 (where n = m - k) piecewise polynomials of degree k - 1 defined over those breakpoints. The elements of this set are called “B-splines”. The vector (t₀, t₁, …, tₘ)’ is referred to as the “knot vector” of the basis and its elements are referred to as “knots”.
At a breakpoint with multiplicity p (i.e. a breakpoint that appears p times in the knot vector), B-splines are guaranteed to have Cᵏ⁻ᵖ⁻¹ continuity.
A B-spline curve using a B-spline basis B, is a parametric curve mapping parameter values in [tₖ₋₁, tₙ₊₁] to a vector space V. For t ∈ [tₖ₋₁, tₙ₊₁] the value of the curve is given by the linear combination of n + 1 control points, pᵢ ∈ V, with the elements of B evaluated at t.
For more information on B-splines and their uses, see (for example) Patrikalakis et al. [1].
[1] https://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node15.html
- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.math.BsplineBasis_[Expression]) -> None
__init__(self: pydrake.math.BsplineBasis_[Expression], order: int, knots: List[pydrake.symbolic.Expression]) -> None
Constructs a B-spline basis with the specified
order
andknots
.- Precondition:
knots
is sorted in non-descending order.
- Raises
RuntimeError if knots.size() < 2 * order. –
__init__(self: pydrake.math.BsplineBasis_[Expression], order: int, num_basis_functions: int, type: pydrake.math.KnotVectorType = <KnotVectorType.kClampedUniform: 1>, initial_parameter_value: pydrake.symbolic.Expression = 0.0, final_parameter_value: pydrake.symbolic.Expression = 1.0) -> None
Constructs a B-spline basis with the specified
order
, num_basis_functions,initial_parameter_value
, final_parameter_value, and an auto-generated knot vector of the specifiedtype
.- Raises
RuntimeError if num_basis_functions < order –
- Precondition:
initial_parameter_value ≤ final_parameter_value
__init__(self: pydrake.math.BsplineBasis_[Expression], other: pydrake.math.BsplineBasis_[Expression]) -> None
- ComputeActiveBasisFunctionIndices(*args, **kwargs)
Overloaded function.
ComputeActiveBasisFunctionIndices(self: pydrake.math.BsplineBasis_[Expression], parameter_interval: Annotated[List[pydrake.symbolic.Expression], FixedSize(2)]) -> List[int]
Returns the indices of the basis functions which may evaluate to non-zero values for some parameter value in
parameter_interval
; all other basis functions are strictly zero overparameter_interval
.- Precondition:
parameter_interval[0] ≤ parameter_interval[1]
- Precondition:
parameter_interval[0] ≥ initial_parameter_value()
- Precondition:
parameter_interval[1] ≤ final_parameter_value()
ComputeActiveBasisFunctionIndices(self: pydrake.math.BsplineBasis_[Expression], parameter_value: pydrake.symbolic.Expression) -> List[int]
Returns the indices of the basis functions which may evaluate to non-zero values for
parameter_value
; all other basis functions are strictly zero at this point.- Precondition:
parameter_value ≥ initial_parameter_value()
- Precondition:
parameter_value ≤ final_parameter_value()
- degree(self: pydrake.math.BsplineBasis_[Expression]) int
The degree of the piecewise polynomials comprising this B-spline basis (k - 1 in the class description).
- EvaluateBasisFunctionI(self: pydrake.math.BsplineBasis_[Expression], i: int, parameter_value: pydrake.symbolic.Expression) pydrake.symbolic.Expression
Returns the value of the
i
-th basis function evaluated atparameter_value
.
- EvaluateCurve(self: pydrake.math.BsplineBasis_[Expression], control_points: List[numpy.ndarray[object[m, 1]]], parameter_value: pydrake.symbolic.Expression) numpy.ndarray[object[m, 1]]
Evaluates the B-spline curve defined by
this
andcontrol_points
at the givenparameter_value
.- Parameter
control_points
: Control points of the B-spline curve.
- Parameter
parameter_value
: Parameter value at which to evaluate the B-spline curve defined by
this
andcontrol_points
.- Precondition:
control_points.size() == num_basis_functions()
- Precondition:
parameter_value ≥ initial_parameter_value()
- Precondition:
parameter_value ≤ final_parameter_value()
- Parameter
- final_parameter_value(self: pydrake.math.BsplineBasis_[Expression]) pydrake.symbolic.Expression
The maximum allowable parameter value for B-spline curves using this basis (tₙ₊₁ in the class description).
- FindContainingInterval(self: pydrake.math.BsplineBasis_[Expression], parameter_value: pydrake.symbolic.Expression) int
For a
parameter_value
= t, the interval that contains it is the pair of knot values [tᵢ, tᵢ₊₁] for the greatest i such that tᵢ ≤ t and tᵢ < final_parameter_value(). This function returns that value of i.- Precondition:
parameter_value ≥ initial_parameter_value()
- Precondition:
parameter_value ≤ final_parameter_value()
- initial_parameter_value(self: pydrake.math.BsplineBasis_[Expression]) pydrake.symbolic.Expression
The minimum allowable parameter value for B-spline curves using this basis (tₖ₋₁ in the class description).
- knots(self: pydrake.math.BsplineBasis_[Expression]) List[pydrake.symbolic.Expression]
The knot vector of this B-spline basis (the vector (t₀, t₁, …, tₘ)’ in the class description).
- num_basis_functions(self: pydrake.math.BsplineBasis_[Expression]) int
The number of basis functions in this B-spline basis (n + 1 in the class description).
- order(self: pydrake.math.BsplineBasis_[Expression]) int
The order of this B-spline basis (k in the class description).
- pydrake.math.CalculateAngularVelocityExpressedInBFromQuaternionDt(*args, **kwargs)
Overloaded function.
CalculateAngularVelocityExpressedInBFromQuaternionDt(quat_AB: pydrake.common.eigen_geometry.Quaternion, quatDt: numpy.ndarray[numpy.float64[4, 1]]) -> numpy.ndarray[numpy.float64[3, 1]]
This function calculates angular velocity from a quaternion and its time- derivative. Algorithm from [Kane, 1983] Section 1.13, Pages 58-59.
[Kane, 1983] “Spacecraft Dynamics,” McGraw-Hill Book Co., New York, 1983. (with P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637
- Parameter
quat_AB
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat_AB is analogous to the rotation matrix R_AB.
- Parameter
quatDt
: Time-derivative of
quat_AB
, i.e. [ẇ, ẋ, ẏ, ż].- Returns
w_AB_B
: B’s angular velocity in A, expressed in B.
CalculateAngularVelocityExpressedInBFromQuaternionDt(quat_AB: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd], quatDt: numpy.ndarray[object[4, 1]]) -> numpy.ndarray[object[3, 1]]
This function calculates angular velocity from a quaternion and its time- derivative. Algorithm from [Kane, 1983] Section 1.13, Pages 58-59.
[Kane, 1983] “Spacecraft Dynamics,” McGraw-Hill Book Co., New York, 1983. (with P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637
- Parameter
quat_AB
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat_AB is analogous to the rotation matrix R_AB.
- Parameter
quatDt
: Time-derivative of
quat_AB
, i.e. [ẇ, ẋ, ẏ, ż].- Returns
w_AB_B
: B’s angular velocity in A, expressed in B.
CalculateAngularVelocityExpressedInBFromQuaternionDt(quat_AB: pydrake.common.eigen_geometry.Quaternion_[Expression], quatDt: numpy.ndarray[object[4, 1]]) -> numpy.ndarray[object[3, 1]]
This function calculates angular velocity from a quaternion and its time- derivative. Algorithm from [Kane, 1983] Section 1.13, Pages 58-59.
[Kane, 1983] “Spacecraft Dynamics,” McGraw-Hill Book Co., New York, 1983. (with P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637
- Parameter
quat_AB
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat_AB is analogous to the rotation matrix R_AB.
- Parameter
quatDt
: Time-derivative of
quat_AB
, i.e. [ẇ, ẋ, ẏ, ż].- Returns
w_AB_B
: B’s angular velocity in A, expressed in B.
- pydrake.math.CalculateQuaternionDtConstraintViolation(*args, **kwargs)
Overloaded function.
CalculateQuaternionDtConstraintViolation(quat: pydrake.common.eigen_geometry.Quaternion, quatDt: numpy.ndarray[numpy.float64[4, 1]]) -> float
This function calculates how well a quaternion and its time-derivative satisfy the quaternion time-derivative constraint specified in [Kane, 1983] Section 1.13, equations 12-13, page 59. For a quaternion [w, x, y, z], the quaternion must satisfy: w^2 + x^2 + y^2 + z^2 = 1, hence its time-derivative must satisfy: 2*(w*ẇ + x*ẋ + y*ẏ + z*ż) = 0.
[Kane, 1983] “Spacecraft Dynamics,” McGraw-Hill Book Co., New York, 1983. (with P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637
- Parameter
quat
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: A quaternion like quat_AB is analogous to the rotation matrix R_AB.
- Parameter
quatDt
: Time-derivative of
quat
, i.e., [ẇ, ẋ, ẏ, ż].- Returns
quaternionDt_constraint_violation
: The amount the time- derivative of the quaternion constraint has been violated, which may be positive or negative (0 means the constraint is perfectly satisfied).
CalculateQuaternionDtConstraintViolation(quat: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd], quatDt: numpy.ndarray[object[4, 1]]) -> pydrake.autodiffutils.AutoDiffXd
This function calculates how well a quaternion and its time-derivative satisfy the quaternion time-derivative constraint specified in [Kane, 1983] Section 1.13, equations 12-13, page 59. For a quaternion [w, x, y, z], the quaternion must satisfy: w^2 + x^2 + y^2 + z^2 = 1, hence its time-derivative must satisfy: 2*(w*ẇ + x*ẋ + y*ẏ + z*ż) = 0.
[Kane, 1983] “Spacecraft Dynamics,” McGraw-Hill Book Co., New York, 1983. (with P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637
- Parameter
quat
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: A quaternion like quat_AB is analogous to the rotation matrix R_AB.
- Parameter
quatDt
: Time-derivative of
quat
, i.e., [ẇ, ẋ, ẏ, ż].- Returns
quaternionDt_constraint_violation
: The amount the time- derivative of the quaternion constraint has been violated, which may be positive or negative (0 means the constraint is perfectly satisfied).
CalculateQuaternionDtConstraintViolation(quat: pydrake.common.eigen_geometry.Quaternion_[Expression], quatDt: numpy.ndarray[object[4, 1]]) -> pydrake.symbolic.Expression
This function calculates how well a quaternion and its time-derivative satisfy the quaternion time-derivative constraint specified in [Kane, 1983] Section 1.13, equations 12-13, page 59. For a quaternion [w, x, y, z], the quaternion must satisfy: w^2 + x^2 + y^2 + z^2 = 1, hence its time-derivative must satisfy: 2*(w*ẇ + x*ẋ + y*ẏ + z*ż) = 0.
[Kane, 1983] “Spacecraft Dynamics,” McGraw-Hill Book Co., New York, 1983. (with P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637
- Parameter
quat
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: A quaternion like quat_AB is analogous to the rotation matrix R_AB.
- Parameter
quatDt
: Time-derivative of
quat
, i.e., [ẇ, ẋ, ẏ, ż].- Returns
quaternionDt_constraint_violation
: The amount the time- derivative of the quaternion constraint has been violated, which may be positive or negative (0 means the constraint is perfectly satisfied).
- pydrake.math.CalculateQuaternionDtFromAngularVelocityExpressedInB(*args, **kwargs)
Overloaded function.
CalculateQuaternionDtFromAngularVelocityExpressedInB(quat_AB: pydrake.common.eigen_geometry.Quaternion, w_AB_B: numpy.ndarray[numpy.float64[3, 1]]) -> numpy.ndarray[numpy.float64[4, 1]]
This function calculates a quaternion’s time-derivative from its quaternion and angular velocity. Algorithm from [Kane, 1983] Section 1.13, Pages 58-59.
[Kane, 1983] “Spacecraft Dynamics,” McGraw-Hill Book Co., New York, 1983. (With P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637
- Parameter
quat_AB
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat_AB is analogous to the rotation matrix R_AB.
- Parameter
w_AB_B
: B’s angular velocity in A, expressed in B.
- Returns
quatDt
: Time-derivative of quat_AB, i.e., [ẇ, ẋ, ẏ, ż].
CalculateQuaternionDtFromAngularVelocityExpressedInB(quat_AB: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd], w_AB_B: numpy.ndarray[object[3, 1]]) -> numpy.ndarray[object[4, 1]]
This function calculates a quaternion’s time-derivative from its quaternion and angular velocity. Algorithm from [Kane, 1983] Section 1.13, Pages 58-59.
[Kane, 1983] “Spacecraft Dynamics,” McGraw-Hill Book Co., New York, 1983. (With P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637
- Parameter
quat_AB
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat_AB is analogous to the rotation matrix R_AB.
- Parameter
w_AB_B
: B’s angular velocity in A, expressed in B.
- Returns
quatDt
: Time-derivative of quat_AB, i.e., [ẇ, ẋ, ẏ, ż].
CalculateQuaternionDtFromAngularVelocityExpressedInB(quat_AB: pydrake.common.eigen_geometry.Quaternion_[Expression], w_AB_B: numpy.ndarray[object[3, 1]]) -> numpy.ndarray[object[4, 1]]
This function calculates a quaternion’s time-derivative from its quaternion and angular velocity. Algorithm from [Kane, 1983] Section 1.13, Pages 58-59.
[Kane, 1983] “Spacecraft Dynamics,” McGraw-Hill Book Co., New York, 1983. (With P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637
- Parameter
quat_AB
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat_AB is analogous to the rotation matrix R_AB.
- Parameter
w_AB_B
: B’s angular velocity in A, expressed in B.
- Returns
quatDt
: Time-derivative of quat_AB, i.e., [ẇ, ẋ, ẏ, ż].
- pydrake.math.ceil(*args, **kwargs)
Overloaded function.
ceil(arg0: float) -> float
ceil(arg0: pydrake.autodiffutils.AutoDiffXd) -> float
ceil(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.ClosestQuaternion(*args, **kwargs)
Overloaded function.
ClosestQuaternion(quat1: pydrake.common.eigen_geometry.Quaternion, quat2: pydrake.common.eigen_geometry.Quaternion) -> pydrake.common.eigen_geometry.Quaternion
Returns a unit quaternion that represents the same orientation as
quat2
, and has the “shortest” geodesic distance on the unit sphere toquat1
.ClosestQuaternion(quat1: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd], quat2: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd]) -> pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd]
Returns a unit quaternion that represents the same orientation as
quat2
, and has the “shortest” geodesic distance on the unit sphere toquat1
.ClosestQuaternion(quat1: pydrake.common.eigen_geometry.Quaternion_[Expression], quat2: pydrake.common.eigen_geometry.Quaternion_[Expression]) -> pydrake.common.eigen_geometry.Quaternion_[Expression]
Returns a unit quaternion that represents the same orientation as
quat2
, and has the “shortest” geodesic distance on the unit sphere toquat1
.
- pydrake.math.ComputeNumericalGradient(calc_func: Callable[[numpy.ndarray[numpy.float64[m, 1]]], numpy.ndarray[numpy.float64[m, 1]]], x: numpy.ndarray[numpy.float64[m, 1]], option: pydrake.math.NumericalGradientOption = <NumericalGradientOption(<NumericalGradientMethod.kForward: 0>)>) numpy.ndarray[numpy.float64[m, n]]
Compute the gradient of a function f(x) through numerical difference.
- Parameter
calc_func
: calc_func(x, &y) computes the value of f(x), and stores the value in y.
calc_func
is responsible for properly resizing the outputy
when it consists of an Eigen vector of Eigen::Dynamic size.- Parameter
x
: The point at which the numerical gradient is computed.
- Parameter
option
: The options for computing numerical gradient.
- Template parameter
DerivedX
: an Eigen column vector.
- Template parameter
DerivedY
: an Eigen column vector.
- Template parameter
DerivedCalcX
: The type of x in the calc_func. Must be an Eigen column vector. It is possible to have DerivedCalcX being different from DerivedX, for example,
calc_func
could be solvers::EvaluatorBase(const Eigen::Ref<const Eigen::VectorXd>&, Eigen::VectorXd*), butx
could be of type Eigen::VectorXd. TODO(hongkai.dai): understand why the default template DerivedCalcX = DerivedX doesn’t compile when I instantiate ComputeNumericalGradient<DerivedX, DerivedY>(calc_func, x);- Returns
gradient
: a matrix of size x.rows() x y.rows(). gradient(i, j) is ∂f(i) / ∂x(j)
Examples:
Click to expand C++ code...
{cc} // Create a std::function from a lambda expression. std::function<void (const Eigen::Vector2d&, Vector3d*)> foo = [](const Eigen::Vector2d& x, Vector3d*y) { (*y)(0) = x(0); (*y)(1) = x(0) * x(1); (*y)(2) = x(0) * std::sin(x(1));}; Eigen::Vector3d x_eval(1, 2, 3); auto J = ComputeNumericalGradient(foo, x_eval); // Note that if we pass in a lambda to ComputeNumericalGradient, then // ComputeNumericalGradient has to instantiate the template types explicitly, // as in this example. The issue of template deduction with std::function is // explained in // https://stackoverflow.com/questions/48529410/template-arguments-deduction-failed-passing-func-pointer-to-stdfunction auto bar = [](const Eigen::Vector2d& x, Eigen::Vector2d* y) {*y = x; }; auto J2 = ComputeNumericalGradient<Eigen::Vector2d, Eigen::Vector2d, Eigen::Vector2d>(bar, Eigen::Vector2d(2, 3));
- Parameter
- pydrake.math.ContinuousAlgebraicRiccatiEquation(A: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], B: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], Q: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], R: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) numpy.ndarray[numpy.float64[m, n]]
Computes the unique stabilizing solution S to the continuous-time algebraic Riccati equation:
\[S A + A' S - S B R^{-1} B' S + Q = 0\]- Raises
RuntimeError if the Hamiltanoian matrix –
Click to expand C++ code...
⌈A BR⁻¹Bᵀ⌉ ⌊Q −Aᵀ⌋
is not invertible.
- Raises
RuntimeError if R is not positive definite. –
Note
the pair (A, B) should be stabilizable, and (Q, A) should be detectable. For more information, please refer to page 526-527 of Linear Systems by Thomas Kailath.
Based on the Matrix Sign Function method outlined in this paper: http://www.engr.iupui.edu/~skoskie/ECE684/Riccati_algorithms.pdf
- pydrake.math.cos(*args, **kwargs)
Overloaded function.
cos(arg0: float) -> float
cos(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
cos(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.cosh(*args, **kwargs)
Overloaded function.
cosh(arg0: float) -> float
cosh(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
cosh(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.DecomposePositiveQuadraticForm(Q: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], b: numpy.ndarray[numpy.float64[m, 1]], c: float, tol: float = 0) Tuple[numpy.ndarray[numpy.float64[m, n]], numpy.ndarray[numpy.float64[m, n]]]
Rewrite a quadratic form xᵀQx + bᵀx + c to (Rx+d)ᵀ(Rx+d) where
Click to expand C++ code...
RᵀR = Q Rᵀd = b / 2 dᵀd = c
This decomposition requires the matrix
Click to expand C++ code...
⌈Q b/2⌉ ⌊bᵀ/2 c⌋
to be positive semidefinite.
We return R and d with the minimal number of rows, namely the rows of R and d equal to the rank of the matrix
Click to expand C++ code...
⌈Q b/2⌉ ⌊bᵀ/2 c⌋
Notice that R might have more rows than Q, For example, the quadratic expression x² + 2x + 5 =(x+1)² + 2², it can be decomposed as
Click to expand C++ code...
⎛⌈1⌉ * x + ⌈1⌉⎞ᵀ * ⎛⌈1⌉ * x + ⌈1⌉⎞ ⎝⌊0⌋ ⌊2⌋⎠ ⎝⌊0⌋ ⌊2⌋⎠
Here R has 2 rows while Q only has 1 row.
On the other hand the quadratic expression x² + 2x + 1 can be decomposed as (x+1) * (x+1), where R has 1 row, same as Q.
Also notice that this decomposition is not unique. For example, with any permutation matrix P, we can define
Click to expand C++ code...
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 constant 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(). R.rows() equals to the rank of the matrix
Click to expand C++ code...
[Q b/2] [bᵀ/2 c]
Precondition: 1. The quadratic form is always non-negative, namely the matrix
Click to expand C++ code...
[Q b/2] [bᵀ/2 c]
is positive semidefinite. 2.
Q
andb
are of the correct size. 3.tol
is non-negative.- Raises
RuntimeError if the precondition is not satisfied. –
- Parameter
- pydrake.math.DecomposePSDmatrixIntoXtransposeTimesX(Y: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], zero_tol: float) numpy.ndarray[numpy.float64[m, n]]
For a symmetric positive semidefinite matrix Y, decompose it into XᵀX, where the number of rows in X equals to the rank of Y. Notice that this decomposition is not unique. For any orthonormal matrix U, s.t UᵀU = identity, X_prime = UX also satisfies X_primeᵀX_prime = Y. Here we only return one valid decomposition.
- Parameter
Y
: A symmetric positive semidefinite matrix.
- Parameter
zero_tol
: We will need to check if some value (for example, the absolute value of Y’s eigenvalues) is smaller than zero_tol. If it is, then we deem that value as 0.
- Returns
X
: . The matrix X satisfies XᵀX = Y and X.rows() = rank(Y).
Precondition: 1. Y is positive semidefinite.
zero_tol is non-negative.
$Raises:
RuntimeError when the pre-conditions are not satisfied.
Note
We only use the lower triangular part of Y.
- Parameter
- pydrake.math.DiscreteAlgebraicRiccatiEquation(*args, **kwargs)
Overloaded function.
DiscreteAlgebraicRiccatiEquation(A: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], B: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], Q: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], R: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) -> numpy.ndarray[numpy.float64[m, n]]
Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation:
AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
- Raises
RuntimeError if Q is not symmetric positive semidefinite. –
RuntimeError if R is not symmetric positive definite. –
RuntimeError if (A, B) isn't a stabilizable pair. –
RuntimeError if (A, C) isn't a detectable pair where Q = CᵀC. –
DiscreteAlgebraicRiccatiEquation(A: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], B: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], Q: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], R: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], N: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) -> numpy.ndarray[numpy.float64[m, n]]
Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation:
AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
This is equivalent to solving the original DARE:
A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
where A₂ and Q₂ are a change of variables:
A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
This overload of the DARE is useful for finding the control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
Click to expand C++ code...
∞ [xₖ]ᵀ[Q N][xₖ] J = Σ [uₖ] [Nᵀ R][uₖ] ΔT k=0
This is a more general form of the following. The linear-quadratic regulator is the feedback control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
Click to expand C++ code...
∞ J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT k=0
This can be refactored as:
Click to expand C++ code...
∞ [xₖ]ᵀ[Q 0][xₖ] J = Σ [uₖ] [0 R][uₖ] ΔT k=0
- Raises
RuntimeError if Q₂ is not symmetric positive semidefinite. –
RuntimeError if R is not symmetric positive definite. –
RuntimeError if (A₂, B) isn't a stabilizable pair. –
RuntimeError if (A₂, C) isn't a detectable pair where Q₂ = CᵀC. –
- pydrake.math.exp(*args, **kwargs)
Overloaded function.
exp(arg0: float) -> float
exp(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
exp(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.ExtractPrincipalSubmatrix(matrix: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], indices: Set[int]) numpy.ndarray[numpy.float64[m, n]]
Extracts the principal submatrix from the ordered set of indices. The indices must be in monotonically increasing order and non-empty. This method makes no assumptions about the symmetry of the matrix, nor that the matrix is square. However, all indices must be valid for both rows and columns.
- pydrake.math.floor(*args, **kwargs)
Overloaded function.
floor(arg0: float) -> float
floor(arg0: pydrake.autodiffutils.AutoDiffXd) -> float
floor(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.inv(*args, **kwargs)
Overloaded function.
inv(arg0: numpy.ndarray[numpy.float64[m, n]]) -> numpy.ndarray[numpy.float64[m, n]]
inv(arg0: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
inv(arg0: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
- pydrake.math.is_quaternion_in_canonical_form(*args, **kwargs)
Overloaded function.
is_quaternion_in_canonical_form(quat: pydrake.common.eigen_geometry.Quaternion) -> bool
This function tests whether a quaternion is in “canonical form” meaning that it tests whether the quaternion [w, x, y, z] has a non-negative w value. Example: [-0.3, +0.4, +0.5, +0.707] is not in canonical form. Example: [+0.3, -0.4, -0.5, -0.707] is in canonical form.
- Parameter
quat
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat is analogous to the rotation matrix R_AB.
- Returns
True
if quat.w() is nonnegative (in canonical form), elseFalse
.
is_quaternion_in_canonical_form(quat: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd]) -> bool
This function tests whether a quaternion is in “canonical form” meaning that it tests whether the quaternion [w, x, y, z] has a non-negative w value. Example: [-0.3, +0.4, +0.5, +0.707] is not in canonical form. Example: [+0.3, -0.4, -0.5, -0.707] is in canonical form.
- Parameter
quat
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat is analogous to the rotation matrix R_AB.
- Returns
True
if quat.w() is nonnegative (in canonical form), elseFalse
.
is_quaternion_in_canonical_form(quat: pydrake.common.eigen_geometry.Quaternion_[Expression]) -> pydrake.symbolic.Formula
This function tests whether a quaternion is in “canonical form” meaning that it tests whether the quaternion [w, x, y, z] has a non-negative w value. Example: [-0.3, +0.4, +0.5, +0.707] is not in canonical form. Example: [+0.3, -0.4, -0.5, -0.707] is in canonical form.
- Parameter
quat
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat is analogous to the rotation matrix R_AB.
- Returns
True
if quat.w() is nonnegative (in canonical form), elseFalse
.
- pydrake.math.IsBothQuaternionAndQuaternionDtOK(*args, **kwargs)
Overloaded function.
IsBothQuaternionAndQuaternionDtOK(quat: pydrake.common.eigen_geometry.Quaternion, quatDt: numpy.ndarray[numpy.float64[4, 1]], tolerance: float) -> bool
This function tests if a quaternion satisfies the time-derivative constraint specified in [Kane, 1983] Section 1.13, equation 13, page 59. A quaternion [w, x, y, z] must satisfy w^2 + x^2 + y^2 + z^2 = 1, hence its time-derivative must satisfy 2*(w*ẇ + x*ẋ + y*ẏ + z*ż) = 0. Note: To accurately test whether the time-derivative quaternion constraint is satisfied, the quaternion constraint is also tested to be accurate.
[Kane, 1983] “Spacecraft Dynamics,” McGraw-Hill Book Co., New York, 1983. (with P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637
- Parameter
quat
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: A quaternion like quat_AB is analogous to the rotation matrix R_AB.
- Parameter
quatDt
: Time-derivative of
quat
, i.e., [ẇ, ẋ, ẏ, ż].- Parameter
tolerance
: Tolerance for quaternion constraints.
- Returns
True
if both of the two previous constraints are within tolerance.
IsBothQuaternionAndQuaternionDtOK(quat: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd], quatDt: numpy.ndarray[object[4, 1]], tolerance: float) -> bool
This function tests if a quaternion satisfies the time-derivative constraint specified in [Kane, 1983] Section 1.13, equation 13, page 59. A quaternion [w, x, y, z] must satisfy w^2 + x^2 + y^2 + z^2 = 1, hence its time-derivative must satisfy 2*(w*ẇ + x*ẋ + y*ẏ + z*ż) = 0. Note: To accurately test whether the time-derivative quaternion constraint is satisfied, the quaternion constraint is also tested to be accurate.
[Kane, 1983] “Spacecraft Dynamics,” McGraw-Hill Book Co., New York, 1983. (with P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637
- Parameter
quat
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: A quaternion like quat_AB is analogous to the rotation matrix R_AB.
- Parameter
quatDt
: Time-derivative of
quat
, i.e., [ẇ, ẋ, ẏ, ż].- Parameter
tolerance
: Tolerance for quaternion constraints.
- Returns
True
if both of the two previous constraints are within tolerance.
IsBothQuaternionAndQuaternionDtOK(quat: pydrake.common.eigen_geometry.Quaternion_[Expression], quatDt: numpy.ndarray[object[4, 1]], tolerance: float) -> pydrake.symbolic.Formula
This function tests if a quaternion satisfies the time-derivative constraint specified in [Kane, 1983] Section 1.13, equation 13, page 59. A quaternion [w, x, y, z] must satisfy w^2 + x^2 + y^2 + z^2 = 1, hence its time-derivative must satisfy 2*(w*ẇ + x*ẋ + y*ẏ + z*ż) = 0. Note: To accurately test whether the time-derivative quaternion constraint is satisfied, the quaternion constraint is also tested to be accurate.
[Kane, 1983] “Spacecraft Dynamics,” McGraw-Hill Book Co., New York, 1983. (with P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637
- Parameter
quat
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: A quaternion like quat_AB is analogous to the rotation matrix R_AB.
- Parameter
quatDt
: Time-derivative of
quat
, i.e., [ẇ, ẋ, ẏ, ż].- Parameter
tolerance
: Tolerance for quaternion constraints.
- Returns
True
if both of the two previous constraints are within tolerance.
- pydrake.math.isnan(*args, **kwargs)
Overloaded function.
isnan(x: float) -> bool
isnan(x: pydrake.autodiffutils.AutoDiffXd) -> bool
isnan(x: pydrake.symbolic.Expression) -> pydrake.symbolic.Formula
- pydrake.math.IsPositiveDefinite(matrix: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], tolerance: float = 0.0) bool
Checks if a matrix is symmetric (with tolerance
symmetry_tolerance
–See also
IsSymmetric) and has all eigenvalues greater than
eigenvalue_tolerance
.eigenvalue_tolerance
must be >= 0 – where 0 implies positive semi-definite (but is of course subject to all of the pitfalls of floating point).To consider the numerical robustness of the eigenvalue estimation, we specifically check that min_eigenvalue >= eigenvalue_tolerance * max(1, max_abs_eigenvalue).
- pydrake.math.IsQuaternionValid(*args, **kwargs)
Overloaded function.
IsQuaternionValid(quat: pydrake.common.eigen_geometry.Quaternion, tolerance: float) -> bool
This function tests if a quaternion satisfies the quaternion constraint specified in [Kane, 1983] Section 1.3, equation 4, page 12, i.e., a quaternion [w, x, y, z] must satisfy: w^2 + x^2 + y^2 + z^2 = 1.
[Kane, 1983] “Spacecraft Dynamics,” McGraw-Hill Book Co., New York, 1983. (with P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637
- Parameter
quat
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: A quaternion like quat_AB is analogous to the rotation matrix R_AB.
- Parameter
tolerance
: Tolerance for quaternion constraint, i.e., how much is w^2 + x^2 + y^2 + z^2 allowed to differ from 1.
- Returns
True
if the quaternion constraint is satisfied within tolerance.
IsQuaternionValid(quat: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd], tolerance: float) -> bool
This function tests if a quaternion satisfies the quaternion constraint specified in [Kane, 1983] Section 1.3, equation 4, page 12, i.e., a quaternion [w, x, y, z] must satisfy: w^2 + x^2 + y^2 + z^2 = 1.
[Kane, 1983] “Spacecraft Dynamics,” McGraw-Hill Book Co., New York, 1983. (with P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637
- Parameter
quat
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: A quaternion like quat_AB is analogous to the rotation matrix R_AB.
- Parameter
tolerance
: Tolerance for quaternion constraint, i.e., how much is w^2 + x^2 + y^2 + z^2 allowed to differ from 1.
- Returns
True
if the quaternion constraint is satisfied within tolerance.
IsQuaternionValid(quat: pydrake.common.eigen_geometry.Quaternion_[Expression], tolerance: float) -> pydrake.symbolic.Formula
This function tests if a quaternion satisfies the quaternion constraint specified in [Kane, 1983] Section 1.3, equation 4, page 12, i.e., a quaternion [w, x, y, z] must satisfy: w^2 + x^2 + y^2 + z^2 = 1.
[Kane, 1983] “Spacecraft Dynamics,” McGraw-Hill Book Co., New York, 1983. (with P. W. Likins and D. A. Levinson). Available for free .pdf download: https://ecommons.cornell.edu/handle/1813/637
- Parameter
quat
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: A quaternion like quat_AB is analogous to the rotation matrix R_AB.
- Parameter
tolerance
: Tolerance for quaternion constraint, i.e., how much is w^2 + x^2 + y^2 + z^2 allowed to differ from 1.
- Returns
True
if the quaternion constraint is satisfied within tolerance.
- pydrake.math.IsSymmetric(*args, **kwargs)
Overloaded function.
IsSymmetric(matrix: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) -> bool
Determines if a matrix is symmetric. If std::equal_to<>()(matrix(i, j), matrix(j, i)) is true for all i, j, then the matrix is symmetric.
IsSymmetric(matrix: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], precision: float) -> bool
Determines if a matrix is symmetric based on whether the difference between matrix(i, j) and matrix(j, i) is smaller than
precision
for all i, j. The precision is absolute. Matrix with nan or inf entries is not allowed.
- class pydrake.math.KnotVectorType
Enum representing types of knot vectors. “Uniform” refers to the spacing between the knots. “Clamped” indicates that the first and last knots have multiplicity equal to the order of the spline.
Reference: http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node17.html
Members:
kUniform :
kClampedUniform :
- __init__(self: pydrake.math.KnotVectorType, value: int) None
- kClampedUniform = <KnotVectorType.kClampedUniform: 1>
- kUniform = <KnotVectorType.kUniform: 0>
- property name
- property value
- pydrake.math.log(*args, **kwargs)
Overloaded function.
log(arg0: float) -> float
log(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
log(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.matmul(*args, **kwargs)
Overloaded function.
matmul(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: numpy.ndarray[numpy.float64[m, n]]) -> numpy.ndarray[numpy.float64[m, n]]
Matrix product for dtype=float @ dtype=float -> dtype=float. The numpy matmul
A @ B
is typically slow when multiplying user-defined dtypes such as AutoDiffXd or Expression. Use this function for better performance, e.g.,matmul(A, B)
. For a dtype=float @ dtype=float, this might be a little slower than numpy, but is provided here for convenience so that the user doesn’t need to be overly careful about the dtype of arguments.matmul(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=float @ dtype=AutoDiffXd -> dtype=AutoDiffXd.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[numpy.float64[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=AutoDiffXd @ dtype=float -> dtype=AutoDiffXd.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=AutoDiffXd @ dtype=AutoDiffXd -> dtype=AutoDiffXd.
matmul(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=float @ dtype=Variable -> dtype=Expression.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[numpy.float64[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Variable @ dtype=float -> dtype=Expression.
matmul(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=float @ dtype=Monomial -> dtype=Polynomial.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[numpy.float64[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Monomial @ dtype=float -> dtype=Polynomial.
matmul(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=float @ dtype=Polynomial -> dtype=Polynomial.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[numpy.float64[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Polynomial @ dtype=float -> dtype=Polynomial.
matmul(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=float @ dtype=Expression -> dtype=Expression.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[numpy.float64[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Expression @ dtype=float -> dtype=Expression.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Variable @ dtype=Variable -> dtype=Expression.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Variable @ dtype=Monomial -> dtype=Polynomial.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Monomial @ dtype=Variable -> dtype=Polynomial.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Variable @ dtype=Polynomial -> dtype=Polynomial.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Polynomial @ dtype=Variable -> dtype=Polynomial.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Variable @ dtype=Expression -> dtype=Expression.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Expression @ dtype=Variable -> dtype=Expression.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Monomial @ dtype=Monomial -> dtype=Polynomial.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Monomial @ dtype=Polynomial -> dtype=Polynomial.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Polynomial @ dtype=Monomial -> dtype=Polynomial.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Monomial @ dtype=Expression -> dtype=Expression.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Expression @ dtype=Monomial -> dtype=Expression.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Polynomial @ dtype=Polynomial -> dtype=Polynomial.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Polynomial @ dtype=Expression -> dtype=Expression.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Expression @ dtype=Polynomial -> dtype=Expression.
matmul(arg0: numpy.ndarray[object[m, n]], arg1: numpy.ndarray[object[m, n]]) -> numpy.ndarray[object[m, n]]
Matrix product for dtype=Expression @ dtype=Expression -> dtype=Expression.
- pydrake.math.max(*args, **kwargs)
Overloaded function.
max(arg0: float, arg1: float) -> float
max(arg0: pydrake.autodiffutils.AutoDiffXd, arg1: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
max(arg0: pydrake.symbolic.Expression, arg1: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.min(*args, **kwargs)
Overloaded function.
min(arg0: float, arg1: float) -> float
min(arg0: pydrake.autodiffutils.AutoDiffXd, arg1: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
min(arg0: pydrake.symbolic.Expression, arg1: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- class pydrake.math.NumericalGradientMethod
Members:
kForward : Compute the gradient as (f(x + Δx) - f(x)) / Δx, with Δx > 0
kBackward : Compute the gradient as (f(x) - f(x - Δx)) / Δx, with Δx > 0
kCentral : Compute the gradient as (f(x + Δx) - f(x - Δx)) / (2Δx), with Δx > 0
- __init__(self: pydrake.math.NumericalGradientMethod, value: int) None
- kBackward = <NumericalGradientMethod.kBackward: 1>
- kCentral = <NumericalGradientMethod.kCentral: 2>
- kForward = <NumericalGradientMethod.kForward: 0>
- property name
- property value
- class pydrake.math.NumericalGradientOption
- __init__(self: pydrake.math.NumericalGradientOption, method: pydrake.math.NumericalGradientMethod, function_accuracy: float = 1e-15) None
- Parameter
function_accuracy
: The accuracy of evaluating function f(x). For double-valued functions (with magnitude around 1), the accuracy is usually about 1E-15.
- Parameter
- NumericalGradientMethod(self: pydrake.math.NumericalGradientOption) pydrake.math.NumericalGradientMethod
- perturbation_size(self: pydrake.math.NumericalGradientOption) float
- pydrake.math.pow(*args, **kwargs)
Overloaded function.
pow(arg0: float, arg1: float) -> float
pow(arg0: pydrake.autodiffutils.AutoDiffXd, arg1: float) -> pydrake.autodiffutils.AutoDiffXd
pow(arg0: pydrake.symbolic.Expression, arg1: float) -> pydrake.symbolic.Expression
pow(arg0: pydrake.symbolic.Expression, arg1: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.QuaternionToCanonicalForm(*args, **kwargs)
Overloaded function.
QuaternionToCanonicalForm(quat: pydrake.common.eigen_geometry.Quaternion) -> pydrake.common.eigen_geometry.Quaternion
This function returns a quaternion in its “canonical form” meaning that it returns a quaternion [w, x, y, z] with a non-negative w. For example, if passed a quaternion [-0.3, +0.4, +0.5, +0.707], the function returns the quaternion’s canonical form [+0.3, -0.4, -0.5, -0.707].
- Parameter
quat
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat is analogous to the rotation matrix R_AB.
- Returns
Canonical form of quat, which means that either the original quat is returned or a quaternion representing the same orientation but with negated [w, x, y, z], to ensure a positive w in returned quaternion.
QuaternionToCanonicalForm(quat: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd]) -> pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd]
This function returns a quaternion in its “canonical form” meaning that it returns a quaternion [w, x, y, z] with a non-negative w. For example, if passed a quaternion [-0.3, +0.4, +0.5, +0.707], the function returns the quaternion’s canonical form [+0.3, -0.4, -0.5, -0.707].
- Parameter
quat
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat is analogous to the rotation matrix R_AB.
- Returns
Canonical form of quat, which means that either the original quat is returned or a quaternion representing the same orientation but with negated [w, x, y, z], to ensure a positive w in returned quaternion.
QuaternionToCanonicalForm(quat: pydrake.common.eigen_geometry.Quaternion_[Expression]) -> pydrake.common.eigen_geometry.Quaternion_[Expression]
This function returns a quaternion in its “canonical form” meaning that it returns a quaternion [w, x, y, z] with a non-negative w. For example, if passed a quaternion [-0.3, +0.4, +0.5, +0.707], the function returns the quaternion’s canonical form [+0.3, -0.4, -0.5, -0.707].
- Parameter
quat
: Quaternion [w, x, y, z] that relates two right-handed orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). Note: quat is analogous to the rotation matrix R_AB.
- Returns
Canonical form of quat, which means that either the original quat is returned or a quaternion representing the same orientation but with negated [w, x, y, z], to ensure a positive w in returned quaternion.
- pydrake.math.RealContinuousLyapunovEquation(A: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], Q: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) numpy.ndarray[numpy.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 [1]. Therefore, if one searches for a Lyapunov function V(z) = zᵀXz for the stable linear system ż = Az, then the solution of the Lyapunov Equation
AᵀX + XA + Q = 0
only returns a valid Lyapunov function if Q is positive semi-definite.The implementation is based on SLICOT routine SB03MD [2]. Note the transformation Q = -C. The complexity of this routine is O(n³). If A is larger than 2-by-2, then a Schur factorization is performed.
- 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.
[1] Bartels, R.H. and G.W. Stewart, “Solution of the Matrix Equation AX + XB = C,” Comm. of the ACM, Vol. 15, No. 9, 1972.
[2] http://slicot.org/objects/software/shared/doc/SB03MD.html
- Parameter
- pydrake.math.RealDiscreteLyapunovEquation(A: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], Q: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) numpy.ndarray[numpy.float64[m, n]]
- Parameter
A
: A user defined real square matrix.
- Parameter
Q
: A user defined real symmetric matrix.
- Precondition:
Q is a symmetric matrix.
Computes the unique solution X to the discrete Lyapunov equation:
AᵀXA - X + Q = 0
, where A is real and square, and Q is real, symmetric and of equal size as A.- Raises
RuntimeError if A or Q are not square matrices or do not have the –
same size. –
Limitations: Given the Eigenvalues of A as λ₁, …, λₙ, there exists a unique solution if and only if λᵢ * λⱼ ≠ 1 ∀ i,j and λᵢ ≠ ±1, ∀ i [1].
- Raises
RuntimeError if the solution is not unique.[3] –
There are no further limitations on the eigenvalues of A. Further, if |λᵢ|<1, ∀ i, and if Q is positive semi-definite, then X is also positive semi-definite [2]. Therefore, if one searches for a Lyapunov function V(z) = zᵀXz for the stable linear system zₙ₊₁ = Azₙ, then the solution of the Lyapunov Equation
AᵀXA - X + Q = 0
only returns a valid Lyapunov function if Q is positive semi-definite.The implementation is based on SLICOT routine SB03MD [2]. Note the transformation Q = -C. The complexity of this routine is O(n³). If A is larger than 2-by-2, then a Schur factorization is performed.
- 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.
[1] Barraud, A.Y., “A numerical algorithm to solve AᵀXA - X = Q,” IEEE® Trans. Auto. Contr., AC-22, pp. 883-885, 1977.
[2] http://slicot.org/objects/software/shared/doc/SB03MD.html
- Parameter
- class pydrake.math.RigidTransform
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 isR_AB
. The monogram notation for the position vector from Ao to Bo isp_AoBo_A
. See multibody_quantities for monogram notation for dynamics.Note
This class does not store the frames associated with the transform and cannot enforce correct usage of this class. For example, it makes sense to multiply RigidTransforms as
X_AB * X_BC
, but notX_AB * X_CB
.Note
This class is not a 4x4 transformation matrix – even though its operator*() methods act mostly like 4x4 matrix multiplication. Instead, this class contains a 3x3 rotation matrix class and a 3x1 position vector. To convert this to a 3x4 matrix, use GetAsMatrix34(). To convert this to a 4x4 matrix, use GetAsMatrix4(). To convert this to an Eigen::Isometry, use GetAsIsometry().
Note
An isometry is sometimes regarded as synonymous with rigid transform. The RigidTransform class has important advantages over Eigen::Isometry. - RigidTransform is built on an underlying rigorous 3x3 RotationMatrix class that has significant functionality for 3D orientation. - In Debug builds, RigidTransform requires a valid 3x3 rotation matrix and a valid (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).
Note
This class is templated; see
RigidTransform_
for the list of instantiations.- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.math.RigidTransform) -> None
Constructs the RigidTransform that corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence, the constructed RigidTransform contains an identity RotationMatrix and a zero position vector.
__init__(self: pydrake.math.RigidTransform, other: pydrake.math.RigidTransform) -> None
__init__(self: pydrake.math.RigidTransform, R: drake::math::RotationMatrix<double>, p: numpy.ndarray[numpy.float64[3, 1]]) -> None
Constructs a RigidTransform from a rotation matrix and a position vector.
- Parameter
R
: rotation matrix relating frames A and B (e.g.,
R_AB
).- Parameter
p
: position vector from frame A’s origin to frame B’s origin, expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
__init__(self: pydrake.math.RigidTransform, rpy: drake::math::RollPitchYaw<double>, p: numpy.ndarray[numpy.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]
.
See also
RotationMatrix::RotationMatrix(const RollPitchYaw<T>&)
- Parameter
p
: position vector from frame A’s origin to frame B’s origin, expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
__init__(self: pydrake.math.RigidTransform, quaternion: pydrake.common.eigen_geometry.Quaternion, p: numpy.ndarray[numpy.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. –
See also
RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&)
__init__(self: pydrake.math.RigidTransform, theta_lambda: pydrake.common.eigen_geometry.AngleAxis, p: numpy.ndarray[numpy.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. –
See also
RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&)
__init__(self: pydrake.math.RigidTransform, R: drake::math::RotationMatrix<double>) -> None
Constructs a RigidTransform with a given RotationMatrix and a zero position vector.
- Parameter
R
: rotation matrix relating frames A and B (e.g.,
R_AB
).
__init__(self: pydrake.math.RigidTransform, p: numpy.ndarray[numpy.float64[3, 1]]) -> None
Constructs a RigidTransform that contains an identity RotationMatrix and a given position vector
p
.- Parameter
p
: position vector from frame A’s origin to frame B’s origin, expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
__init__(self: pydrake.math.RigidTransform, pose: pydrake.common.eigen_geometry.Isometry3) -> None
Constructs a RigidTransform from an Eigen Isometry3.
- Parameter
pose
: Isometry3 that contains an allegedly valid rotation matrix
R_AB
and also contains a position vectorp_AoBo_A
from frame A’s origin to frame B’s origin.p_AoBo_A
must be expressed in frame A.
- Raises
RuntimeError in debug builds if R_AB is not a proper orthonormal –
3x3 rotation matrix. –
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix().__init__(self: pydrake.math.RigidTransform, pose: numpy.ndarray[numpy.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.
Click to expand C++ code...
const Vector3<double> position(4, 5, 6); const RigidTransform<double> X1(3 * position); ---------------------------------------------- const RotationMatrix<double> R(RollPitchYaw<double>(1, 2, 3)); Eigen::Matrix<double, 3, 4> pose34; pose34 << R.matrix(), position; const RigidTransform<double> X2(1.0 * pose34); ---------------------------------------------- Eigen::Matrix<double, 4, 4> pose4; pose4 << R.matrix(), position, 0, 0, 0, 1; const RigidTransform<double> X3(pose4 * pose4);
- template cast
Instantiations:
cast[float]
,cast[AutoDiffXd]
,cast[Expression]
- cast[AutoDiffXd](self: pydrake.math.RigidTransform) 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,
Click to expand C++ code...
RigidTransform<double> source = RigidTransform<double>::Identity(); RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();
- Template parameter
U
: Scalar type on which the returned RigidTransform is templated.
Note
RigidTransform<From>::cast<To>()
creates a newRigidTransform<To>
from aRigidTransform<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not vice-versa.- Template parameter
- cast[Expression](self: pydrake.math.RigidTransform) 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,
Click to expand C++ code...
RigidTransform<double> source = RigidTransform<double>::Identity(); RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();
- Template parameter
U
: Scalar type on which the returned RigidTransform is templated.
Note
RigidTransform<From>::cast<To>()
creates a newRigidTransform<To>
from aRigidTransform<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not vice-versa.- Template parameter
- cast[float](self: pydrake.math.RigidTransform) pydrake.math.RigidTransform
Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T. For example,
Click to expand C++ code...
RigidTransform<double> source = RigidTransform<double>::Identity(); RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();
- Template parameter
U
: Scalar type on which the returned RigidTransform is templated.
Note
RigidTransform<From>::cast<To>()
creates a newRigidTransform<To>
from aRigidTransform<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not vice-versa.- Template parameter
- GetAsIsometry3(self: pydrake.math.RigidTransform) pydrake.common.eigen_geometry.Isometry3
Returns the isometry in ℜ³ that is equivalent to a RigidTransform.
- GetAsMatrix34(self: pydrake.math.RigidTransform) numpy.ndarray[numpy.float64[3, 4]]
Returns the 3x4 matrix associated with this RigidTransform, i.e., X_AB.
Click to expand C++ code...
┌ ┐ │ R_AB p_AoBo_A │ └ ┘
- GetAsMatrix4(self: pydrake.math.RigidTransform) numpy.ndarray[numpy.float64[4, 4]]
Returns the 4x4 matrix associated with this RigidTransform, i.e., X_AB.
Click to expand C++ code...
┌ ┐ │ R_AB p_AoBo_A │ │ │ │ 0 1 │ └ ┘
- GetMaximumAbsoluteDifference(self: pydrake.math.RigidTransform, other: pydrake.math.RigidTransform) float
Computes the infinity norm of
this
- other (i.e., the maximum absolute value of the difference between the elements ofthis
andother
).- Parameter
other
: RigidTransform to subtract from
this
.
- Returns
‖this - other‖∞
- Parameter
- GetMaximumAbsoluteTranslationDifference(self: pydrake.math.RigidTransform, other: pydrake.math.RigidTransform) float
Returns the maximum absolute value of the difference in the position vectors (translation) in
this
andother
. In other words, returns the infinity norm of the difference in the position vectors.- Parameter
other
: RigidTransform whose position vector is subtracted from the position vector in
this
.
- Parameter
- static Identity() pydrake.math.RigidTransform
Returns the identity RigidTransform (corresponds to coincident frames).
- Returns
the RigidTransform that corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence, the returned RigidTransform contains a 3x3 identity matrix and a zero position vector.
- inverse(self: pydrake.math.RigidTransform) pydrake.math.RigidTransform
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.
- InvertAndCompose(self: pydrake.math.RigidTransform, other: pydrake.math.RigidTransform) pydrake.math.RigidTransform
Calculates the product of
this
inverted and another RigidTransform. If you considerthis
to be the transform X_AB, andother
to be X_AC, then this method returns X_BC = X_AB⁻¹ * X_AC. For T==double, this method can be much faster than inverting first and then performing the composition, because it can take advantage of the special structure of a rigid transform to avoid unnecessary memory and floating point operations. On some platforms it can use SIMD instructions for further speedups.- Parameter
other
: RigidTransform that post-multiplies
this
inverted.- Returns
X_BC
: where X_BC = this⁻¹ * other.
Note
It is possible (albeit improbable) to create an invalid rigid transform by accumulating round-off error with a large number of multiplies.
- Parameter
- IsExactlyEqualTo(self: pydrake.math.RigidTransform, other: pydrake.math.RigidTransform) bool
Returns true if
this
is exactly equal toother
.- Parameter
other
: RigidTransform to compare to
this
.
- Returns
True
if each element ofthis
is exactly equal to the corresponding element ofother
.
- Parameter
- IsExactlyIdentity(self: pydrake.math.RigidTransform) bool
Returns
True
ifthis
is exactly the identity RigidTransform.See also
IsNearlyIdentity().
- IsNearlyEqualTo(self: pydrake.math.RigidTransform, other: pydrake.math.RigidTransform, tolerance: float) bool
Compares each element of
this
to the corresponding element ofother
to check if they are the same to within a specifiedtolerance
.- Parameter
other
: RigidTransform to compare to
this
.- Parameter
tolerance
: maximum allowable absolute difference between the elements in
this
andother
.
- Returns
True
if‖this.matrix() - other.matrix()‖∞ <= tolerance
.
Note
Consider scaling tolerance with the largest of magA and magB, where magA and magB denoted the magnitudes of
this
position vector andother
position vectors, respectively.- Parameter
- IsNearlyIdentity(self: pydrake.math.RigidTransform, translation_tolerance: float) bool
Returns true if
this
is within tolerance of the identity RigidTransform.- Parameter
translation_tolerance
: a non-negative number. One way to choose
translation_tolerance
is to multiply a characteristic length (e.g., the magnitude of a characteristic position vector) by an epsilon (e.g., RotationMatrix::get_internal_tolerance_for_orthonormality()).
- Returns
True
if the RotationMatrix portion ofthis
satisfies RotationMatrix::IsNearlyIdentity() and if the position vector portion ofthis
is equal to zero vector withintranslation_tolerance
.
See also
IsExactlyIdentity().
- Parameter
- multiply(*args, **kwargs)
Overloaded function.
multiply(self: pydrake.math.RigidTransform, other: pydrake.math.RigidTransform) -> pydrake.math.RigidTransform
Multiplies
this
RigidTransformX_AB
by theother
RigidTransformX_BC
and returns the RigidTransformX_AC = X_AB * X_BC
.multiply(self: pydrake.math.RigidTransform, p_BoQ_B: numpy.ndarray[numpy.float64[3, 1]]) -> numpy.ndarray[numpy.float64[3, 1]]
Multiplies
this
RigidTransformX_AB
by the position vectorp_BoQ_B
which is from Bo (B’s origin) to an arbitrary point Q.- Parameter
p_BoQ_B
: position vector from Bo to Q, expressed in frame B.
- Returns
p_AoQ_A
: position vector from Ao to Q, expressed in frame A.
multiply(self: pydrake.math.RigidTransform, vec_B: numpy.ndarray[numpy.float64[4, 1]]) -> numpy.ndarray[numpy.float64[4, 1]]
Multiplies
this
RigidTransformX_AB
by the 4-element vectorvec_B
, equivalent toX_AB.GetAsMatrix4() * vec_B
.- Parameter
vec_B
: 4-element vector whose first 3 elements are the position vector p_BoQ_B from Bo (frame B’s origin) to an arbitrary point Q, expressed in frame B and whose 4ᵗʰ element is 1 𝐨𝐫 whose first 3 elements are a vector (maybe unrelated to Bo or Q) and whose 4ᵗʰ element is 0.
- Returns
vec_A
: 4-element vector whose first 3 elements are the position vector p_AoQ_A from Ao (frame A’s origin) to Q, expressed in frame A and whose 4ᵗʰ element is 1 𝐨𝐫 whose first 3 elements are a vector (maybe unrelated to Bo and Q) and whose 4ᵗʰ element is 0.
- Raises
RuntimeError if the 4ᵗʰ element of vec_B is not 0 or 1. –
multiply(self: pydrake.math.RigidTransform, p_BoQ_B: numpy.ndarray[numpy.float64[3, n]]) -> numpy.ndarray[numpy.float64[3, n]]
Multiplies
this
RigidTransformX_AB
by the n position vectorsp_BoQ1_B
… p_BoQn_B, wherep_BoQi_B
is the iᵗʰ position vector from Bo (frame B’s origin) to an arbitrary point Qi, expressed in frame B.- Parameter
p_BoQ_B
: 3 x n
matrix with n position vectorsp_BoQi_B
or an expression that resolves to a3 x n
matrix of position vectors.- Returns
p_AoQ_A
: 3 x n
matrix with n position vectorsp_AoQi_A
, i.e., n position vectors from Ao (frame A’s origin) to Qi, expressed in frame A. Specifically, this operator* is defined so thatX_AB * p_BoQ_B
returnsp_AoQ_A = p_AoBo_A + R_AB * p_BoQ_B
, wherep_AoBo_A
is the position vector from Ao to Bo expressed in A andR_AB
is the rotation matrix relating the orientation of frames A and B.
Note
As needed, use parentheses. This operator* is not associative. To see this, let
p = p_AoBo_A
, q = p_BoQ_B and note (X_AB * q) * 7 = (p + R_AB * q) * 7 ≠ X_AB * (q * 7) = p + R_AB * (q * 7).Click to expand C++ code...
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) drake::math::RotationMatrix<double>
Returns R_AB, the rotation matrix portion of
this
RigidTransform.- Returns
R_AB
: the rotation matrix portion of
this
RigidTransform.
- Returns
- set(self: pydrake.math.RigidTransform, R: drake::math::RotationMatrix<double>, p: numpy.ndarray[numpy.float64[3, 1]]) None
Sets
this
RigidTransform from a RotationMatrix and a position vector.- Parameter
R
: rotation matrix relating frames A and B (e.g.,
R_AB
).- Parameter
p
: position vector from frame A’s origin to frame B’s origin, expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
- Parameter
- set_rotation(*args, **kwargs)
Overloaded function.
set_rotation(self: pydrake.math.RigidTransform, R: drake::math::RotationMatrix<double>) -> None
Sets the RotationMatrix portion of
this
RigidTransform.- Parameter
R
: rotation matrix relating frames A and B (e.g.,
R_AB
).
set_rotation(self: pydrake.math.RigidTransform, rpy: drake::math::RollPitchYaw<double>) -> None
Sets the rotation part of
this
RigidTransform from a RollPitchYaw.- Parameter
rpy
: “roll-pitch-yaw” angles.
See also
RotationMatrix::RotationMatrix(const RollPitchYaw<T>&) which describes the parameter, preconditions, etc.
set_rotation(self: pydrake.math.RigidTransform, quaternion: pydrake.common.eigen_geometry.Quaternion) -> None
Sets the rotation part of
this
RigidTransform from a Quaternion.- Parameter
quaternion
: a quaternion which may or may not have unit length.
See also
RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&) which describes the parameter, preconditions, exception conditions, etc.
set_rotation(self: pydrake.math.RigidTransform, theta_lambda: pydrake.common.eigen_geometry.AngleAxis) -> None
Sets the rotation part of
this
RigidTransform from an AngleAxis.- Parameter
theta_lambda
: an angle
theta
(in radians) and vectorlambda
.
See also
RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&) which describes the parameter, preconditions, exception conditions, etc.
- set_translation(self: pydrake.math.RigidTransform, p: numpy.ndarray[numpy.float64[3, 1]]) None
Sets the position vector portion of
this
RigidTransform.- Parameter
p
: position vector from Ao (frame A’s origin) to Bo (frame B’s origin) expressed in frame A. In monogram notation p is denoted p_AoBo_A.
- Parameter
- SetFromIsometry3(self: pydrake.math.RigidTransform, pose: pydrake.common.eigen_geometry.Isometry3) None
Sets
this
RigidTransform from an Eigen Isometry3.- Parameter
pose
: Isometry3 that contains an allegedly valid rotation matrix
R_AB
and also contains a position vectorp_AoBo_A
from frame A’s origin to frame B’s origin.p_AoBo_A
must be expressed in frame A.
- Raises
RuntimeError in debug builds if R_AB is not a proper orthonormal –
3x3 rotation matrix. –
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix().- Parameter
- SetIdentity(self: pydrake.math.RigidTransform) pydrake.math.RigidTransform
Sets
this
RigidTransform so it corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence,this
RigidTransform contains a 3x3 identity matrix and a zero position vector.
- translation(self: pydrake.math.RigidTransform) numpy.ndarray[numpy.float64[3, 1]]
Returns
p_AoBo_A
, the position vector portion ofthis
RigidTransform, i.e., position vector from Ao (frame A’s origin) to Bo (frame B’s origin).
- template pydrake.math.RigidTransform_
Instantiations:
RigidTransform_[float]
,RigidTransform_[AutoDiffXd]
,RigidTransform_[Expression]
- 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 isR_AB
. The monogram notation for the position vector from Ao to Bo isp_AoBo_A
. See multibody_quantities for monogram notation for dynamics.Note
This class does not store the frames associated with the transform and cannot enforce correct usage of this class. For example, it makes sense to multiply RigidTransforms as
X_AB * X_BC
, but notX_AB * X_CB
.Note
This class is not a 4x4 transformation matrix – even though its operator*() methods act mostly like 4x4 matrix multiplication. Instead, this class contains a 3x3 rotation matrix class and a 3x1 position vector. To convert this to a 3x4 matrix, use GetAsMatrix34(). To convert this to a 4x4 matrix, use GetAsMatrix4(). To convert this to an Eigen::Isometry, use GetAsIsometry().
Note
An isometry is sometimes regarded as synonymous with rigid transform. The RigidTransform class has important advantages over Eigen::Isometry. - RigidTransform is built on an underlying rigorous 3x3 RotationMatrix class that has significant functionality for 3D orientation. - In Debug builds, RigidTransform requires a valid 3x3 rotation matrix and a valid (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).
- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.math.RigidTransform_[AutoDiffXd]) -> None
Constructs the RigidTransform that corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence, the constructed RigidTransform contains an identity RotationMatrix and a zero position vector.
__init__(self: pydrake.math.RigidTransform_[AutoDiffXd], other: pydrake.math.RigidTransform_[AutoDiffXd]) -> None
__init__(self: pydrake.math.RigidTransform_[AutoDiffXd], R: drake::math::RotationMatrix<Eigen::AutoDiffScalar<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >, p: numpy.ndarray[object[3, 1]]) -> None
Constructs a RigidTransform from a rotation matrix and a position vector.
- Parameter
R
: rotation matrix relating frames A and B (e.g.,
R_AB
).- Parameter
p
: position vector from frame A’s origin to frame B’s origin, expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
__init__(self: pydrake.math.RigidTransform_[AutoDiffXd], rpy: drake::math::RollPitchYaw<Eigen::AutoDiffScalar<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >, p: numpy.ndarray[object[3, 1]]) -> None
Constructs a RigidTransform from a RollPitchYaw and a position vector.
- Parameter
rpy
: a RollPitchYaw which is a 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]
.
See also
RotationMatrix::RotationMatrix(const RollPitchYaw<T>&)
- Parameter
p
: position vector from frame A’s origin to frame B’s origin, expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
__init__(self: pydrake.math.RigidTransform_[AutoDiffXd], quaternion: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd], p: numpy.ndarray[object[3, 1]]) -> None
Constructs a RigidTransform from a Quaternion and a position vector.
- Parameter
quaternion
: a 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. –
See also
RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&)
__init__(self: pydrake.math.RigidTransform_[AutoDiffXd], theta_lambda: pydrake.common.eigen_geometry.AngleAxis_[AutoDiffXd], p: numpy.ndarray[object[3, 1]]) -> None
Constructs a RigidTransform from an AngleAxis and a position vector.
- Parameter
theta_lambda
: an Eigen::AngleAxis whose associated axis (vector direction herein called
lambda
) is 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. –
See also
RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&)
__init__(self: pydrake.math.RigidTransform_[AutoDiffXd], R: drake::math::RotationMatrix<Eigen::AutoDiffScalar<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >) -> None
Constructs a RigidTransform with a given RotationMatrix and a zero position vector.
- Parameter
R
: rotation matrix relating frames A and B (e.g.,
R_AB
).
__init__(self: pydrake.math.RigidTransform_[AutoDiffXd], p: numpy.ndarray[object[3, 1]]) -> None
Constructs a RigidTransform that contains an identity RotationMatrix and a given position vector
p
.- Parameter
p
: position vector from frame A’s origin to frame B’s origin, expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
__init__(self: pydrake.math.RigidTransform_[AutoDiffXd], pose: pydrake.common.eigen_geometry.Isometry3_[AutoDiffXd]) -> None
Constructs a RigidTransform from an Eigen Isometry3.
- Parameter
pose
: Isometry3 that contains an allegedly valid rotation matrix
R_AB
and also contains a position vectorp_AoBo_A
from frame A’s origin to frame B’s origin.p_AoBo_A
must be expressed in frame A.
- Raises
RuntimeError in debug builds if R_AB is not a proper orthonormal –
3x3 rotation matrix. –
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix().__init__(self: pydrake.math.RigidTransform_[AutoDiffXd], pose: numpy.ndarray[object[m, n]]) -> None
Constructs a RigidTransform from an appropriate Eigen expression.
- Parameter
pose
: Generic Eigen matrix expression.
- Raises
RuntimeError if the Eigen expression in pose does not resolve –
to a Vector3 or 3x4 matrix or 4x4 matrix or if the rotational part –
of pose is not a proper orthonormal 3x3 rotation matrix or if –
pose` is a 4x4 matrix whose final row is not [0, 0, 0, 1] –
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix().Note
This constructor prevents ambiguity that would otherwise exist for a RigidTransform constructor whose argument is an Eigen expression.
Click to expand C++ code...
const Vector3<double> position(4, 5, 6); const RigidTransform<double> X1(3 * position); ---------------------------------------------- const RotationMatrix<double> R(RollPitchYaw<double>(1, 2, 3)); Eigen::Matrix<double, 3, 4> pose34; pose34 << R.matrix(), position; const RigidTransform<double> X2(1.0 * pose34); ---------------------------------------------- Eigen::Matrix<double, 4, 4> pose4; pose4 << R.matrix(), position, 0, 0, 0, 1; const RigidTransform<double> X3(pose4 * pose4);
- template cast
Instantiations:
cast[AutoDiffXd]
- cast[AutoDiffXd](self: pydrake.math.RigidTransform_[AutoDiffXd]) pydrake.math.RigidTransform_[AutoDiffXd]
Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T. For example,
Click to expand C++ code...
RigidTransform<double> source = RigidTransform<double>::Identity(); RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();
- Template parameter
U
: Scalar type on which the returned RigidTransform is templated.
Note
RigidTransform<From>::cast<To>()
creates a newRigidTransform<To>
from aRigidTransform<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not vice-versa.- Template parameter
- GetAsIsometry3(self: pydrake.math.RigidTransform_[AutoDiffXd]) pydrake.common.eigen_geometry.Isometry3_[AutoDiffXd]
Returns the isometry in ℜ³ that is equivalent to a RigidTransform.
- GetAsMatrix34(self: pydrake.math.RigidTransform_[AutoDiffXd]) numpy.ndarray[object[3, 4]]
Returns the 3x4 matrix associated with this RigidTransform, i.e., X_AB.
Click to expand C++ code...
┌ ┐ │ 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.
Click to expand C++ code...
┌ ┐ │ R_AB p_AoBo_A │ │ │ │ 0 1 │ └ ┘
- GetMaximumAbsoluteDifference(self: pydrake.math.RigidTransform_[AutoDiffXd], other: pydrake.math.RigidTransform_[AutoDiffXd]) pydrake.autodiffutils.AutoDiffXd
Computes the infinity norm of
this
- other (i.e., the maximum absolute value of the difference between the elements ofthis
andother
).- Parameter
other
: RigidTransform to subtract from
this
.
- Returns
‖this - other‖∞
- Parameter
- GetMaximumAbsoluteTranslationDifference(self: pydrake.math.RigidTransform_[AutoDiffXd], other: pydrake.math.RigidTransform_[AutoDiffXd]) pydrake.autodiffutils.AutoDiffXd
Returns the maximum absolute value of the difference in the position vectors (translation) in
this
andother
. In other words, returns the infinity norm of the difference in the position vectors.- Parameter
other
: RigidTransform whose position vector is subtracted from the position vector in
this
.
- Parameter
- static Identity() pydrake.math.RigidTransform_[AutoDiffXd]
Returns the identity RigidTransform (corresponds to coincident frames).
- Returns
the RigidTransform that corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence, the returned RigidTransform contains a 3x3 identity matrix and a zero position vector.
- inverse(self: pydrake.math.RigidTransform_[AutoDiffXd]) pydrake.math.RigidTransform_[AutoDiffXd]
Returns X_BA = X_AB⁻¹, the inverse of
this
RigidTransform.Note
The inverse of RigidTransform X_AB is X_BA, which contains the rotation matrix R_BA = R_AB⁻¹ = R_ABᵀ and the position vector
p_BoAo_B_
(position from B’s origin Bo to A’s origin Ao, expressed in frame B).Note
: The 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.
- InvertAndCompose(self: pydrake.math.RigidTransform_[AutoDiffXd], other: pydrake.math.RigidTransform_[AutoDiffXd]) pydrake.math.RigidTransform_[AutoDiffXd]
Calculates the product of
this
inverted and another RigidTransform. If you considerthis
to be the transform X_AB, andother
to be X_AC, then this method returns X_BC = X_AB⁻¹ * X_AC. For T==double, this method can be much faster than inverting first and then performing the composition, because it can take advantage of the special structure of a rigid transform to avoid unnecessary memory and floating point operations. On some platforms it can use SIMD instructions for further speedups.- Parameter
other
: RigidTransform that post-multiplies
this
inverted.- Returns
X_BC
: where X_BC = this⁻¹ * other.
Note
It is possible (albeit improbable) to create an invalid rigid transform by accumulating round-off error with a large number of multiplies.
- Parameter
- IsExactlyEqualTo(self: pydrake.math.RigidTransform_[AutoDiffXd], other: pydrake.math.RigidTransform_[AutoDiffXd]) bool
Returns true if
this
is exactly equal toother
.- Parameter
other
: RigidTransform to compare to
this
.
- Returns
True
if each element ofthis
is exactly equal to the corresponding element ofother
.
- Parameter
- IsExactlyIdentity(self: pydrake.math.RigidTransform_[AutoDiffXd]) bool
Returns
True
ifthis
is exactly the identity RigidTransform.See also
IsNearlyIdentity().
- IsNearlyEqualTo(self: pydrake.math.RigidTransform_[AutoDiffXd], other: pydrake.math.RigidTransform_[AutoDiffXd], tolerance: float) bool
Compares each element of
this
to the corresponding element ofother
to check if they are the same to within a specifiedtolerance
.- Parameter
other
: RigidTransform to compare to
this
.- Parameter
tolerance
: maximum allowable absolute difference between the elements in
this
andother
.
- Returns
True
if‖this.matrix() - other.matrix()‖∞ <= tolerance
.
Note
Consider scaling tolerance with the largest of magA and magB, where magA and magB denoted the magnitudes of
this
position vector andother
position vectors, respectively.- Parameter
- IsNearlyIdentity(self: pydrake.math.RigidTransform_[AutoDiffXd], translation_tolerance: float) bool
Returns true if
this
is within tolerance of the identity RigidTransform.- Parameter
translation_tolerance
: a non-negative number. One way to choose
translation_tolerance
is to multiply a characteristic length (e.g., the magnitude of a characteristic position vector) by an epsilon (e.g., RotationMatrix::get_internal_tolerance_for_orthonormality()).
- Returns
True
if the RotationMatrix portion ofthis
satisfies RotationMatrix::IsNearlyIdentity() and if the position vector portion ofthis
is equal to zero vector withintranslation_tolerance
.
See also
IsExactlyIdentity().
- Parameter
- multiply(*args, **kwargs)
Overloaded function.
multiply(self: pydrake.math.RigidTransform_[AutoDiffXd], other: pydrake.math.RigidTransform_[AutoDiffXd]) -> pydrake.math.RigidTransform_[AutoDiffXd]
Multiplies
this
RigidTransformX_AB
by theother
RigidTransformX_BC
and returns the RigidTransformX_AC = X_AB * X_BC
.multiply(self: pydrake.math.RigidTransform_[AutoDiffXd], p_BoQ_B: numpy.ndarray[object[3, 1]]) -> numpy.ndarray[object[3, 1]]
Multiplies
this
RigidTransformX_AB
by the position vectorp_BoQ_B
which is from Bo (B’s origin) to an arbitrary point Q.- Parameter
p_BoQ_B
: position vector from Bo to Q, expressed in frame B.
- Returns
p_AoQ_A
: position vector from Ao to Q, expressed in frame A.
multiply(self: pydrake.math.RigidTransform_[AutoDiffXd], vec_B: numpy.ndarray[object[4, 1]]) -> numpy.ndarray[object[4, 1]]
Multiplies
this
RigidTransformX_AB
by the 4-element vectorvec_B
, equivalent toX_AB.GetAsMatrix4() * vec_B
.- Parameter
vec_B
: 4-element vector whose first 3 elements are the position vector p_BoQ_B from Bo (frame B’s origin) to an arbitrary point Q, expressed in frame B and whose 4ᵗʰ element is 1 𝐨𝐫 whose first 3 elements are a vector (maybe unrelated to Bo or Q) and whose 4ᵗʰ element is 0.
- Returns
vec_A
: 4-element vector whose first 3 elements are the position vector p_AoQ_A from Ao (frame A’s origin) to Q, expressed in frame A and whose 4ᵗʰ element is 1 𝐨𝐫 whose first 3 elements are a vector (maybe unrelated to Bo and Q) and whose 4ᵗʰ element is 0.
- Raises
RuntimeError if the 4ᵗʰ element of vec_B is not 0 or 1. –
multiply(self: pydrake.math.RigidTransform_[AutoDiffXd], p_BoQ_B: numpy.ndarray[object[3, n]]) -> numpy.ndarray[object[3, n]]
Multiplies
this
RigidTransformX_AB
by the n position vectorsp_BoQ1_B
… p_BoQn_B, wherep_BoQi_B
is the iᵗʰ position vector from Bo (frame B’s origin) to an arbitrary point Qi, expressed in frame B.- Parameter
p_BoQ_B
: 3 x n
matrix with n position vectorsp_BoQi_B
or an expression that resolves to a3 x n
matrix of position vectors.- Returns
p_AoQ_A
: 3 x n
matrix with n position vectorsp_AoQi_A
, i.e., n position vectors from Ao (frame A’s origin) to Qi, expressed in frame A. Specifically, this operator* is defined so thatX_AB * p_BoQ_B
returnsp_AoQ_A = p_AoBo_A + R_AB * p_BoQ_B
, wherep_AoBo_A
is the position vector from Ao to Bo expressed in A andR_AB
is the rotation matrix relating the orientation of frames A and B.
Note
As needed, use parentheses. This operator* is not associative. To see this, let
p = p_AoBo_A
, q = p_BoQ_B and note (X_AB * q) * 7 = (p + R_AB * q) * 7 ≠ X_AB * (q * 7) = p + R_AB * (q * 7).Click to expand C++ code...
const RollPitchYaw<double> rpy(0.1, 0.2, 0.3); const RigidTransform<double> X_AB(rpy, Vector3d(1, 2, 3)); Eigen::Matrix<double, 3, 2> p_BoQ_B; p_BoQ_B.col(0) = Vector3d(4, 5, 6); p_BoQ_B.col(1) = Vector3d(9, 8, 7); const Eigen::Matrix<double, 3, 2> p_AoQ_A = X_AB * p_BoQ_B;
- rotation(self: pydrake.math.RigidTransform_[AutoDiffXd]) drake::math::RotationMatrix<Eigen::AutoDiffScalar<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >
Returns R_AB, the rotation matrix portion of
this
RigidTransform.- Returns
R_AB
: the rotation matrix portion of
this
RigidTransform.
- Returns
- set(self: pydrake.math.RigidTransform_[AutoDiffXd], R: drake::math::RotationMatrix<Eigen::AutoDiffScalar<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >, p: numpy.ndarray[object[3, 1]]) None
Sets
this
RigidTransform from a RotationMatrix and a position vector.- Parameter
R
: rotation matrix relating frames A and B (e.g.,
R_AB
).- Parameter
p
: position vector from frame A’s origin to frame B’s origin, expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
- Parameter
- set_rotation(*args, **kwargs)
Overloaded function.
set_rotation(self: pydrake.math.RigidTransform_[AutoDiffXd], R: drake::math::RotationMatrix<Eigen::AutoDiffScalar<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >) -> None
Sets the RotationMatrix portion of
this
RigidTransform.- Parameter
R
: rotation matrix relating frames A and B (e.g.,
R_AB
).
set_rotation(self: pydrake.math.RigidTransform_[AutoDiffXd], rpy: drake::math::RollPitchYaw<Eigen::AutoDiffScalar<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >) -> None
Sets the rotation part of
this
RigidTransform from a RollPitchYaw.- Parameter
rpy
: “roll-pitch-yaw” angles.
See also
RotationMatrix::RotationMatrix(const RollPitchYaw<T>&) which describes the parameter, preconditions, etc.
set_rotation(self: pydrake.math.RigidTransform_[AutoDiffXd], quaternion: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd]) -> None
Sets the rotation part of
this
RigidTransform from a Quaternion.- Parameter
quaternion
: a quaternion which may or may not have unit length.
See also
RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&) which describes the parameter, preconditions, exception conditions, etc.
set_rotation(self: pydrake.math.RigidTransform_[AutoDiffXd], theta_lambda: pydrake.common.eigen_geometry.AngleAxis_[AutoDiffXd]) -> None
Sets the rotation part of
this
RigidTransform from an AngleAxis.- Parameter
theta_lambda
: an angle
theta
(in radians) and vectorlambda
.
See also
RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&) which describes the parameter, preconditions, exception conditions, etc.
- set_translation(self: pydrake.math.RigidTransform_[AutoDiffXd], p: numpy.ndarray[object[3, 1]]) None
Sets the position vector portion of
this
RigidTransform.- Parameter
p
: position vector from Ao (frame A’s origin) to Bo (frame B’s origin) expressed in frame A. In monogram notation p is denoted p_AoBo_A.
- Parameter
- SetFromIsometry3(self: pydrake.math.RigidTransform_[AutoDiffXd], pose: pydrake.common.eigen_geometry.Isometry3_[AutoDiffXd]) None
Sets
this
RigidTransform from an Eigen Isometry3.- Parameter
pose
: Isometry3 that contains an allegedly valid rotation matrix
R_AB
and also contains a position vectorp_AoBo_A
from frame A’s origin to frame B’s origin.p_AoBo_A
must be expressed in frame A.
- Raises
RuntimeError in debug builds if R_AB is not a proper orthonormal –
3x3 rotation matrix. –
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix().- Parameter
- SetIdentity(self: pydrake.math.RigidTransform_[AutoDiffXd]) pydrake.math.RigidTransform_[AutoDiffXd]
Sets
this
RigidTransform so it corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence,this
RigidTransform contains a 3x3 identity matrix and a zero position vector.
- translation(self: pydrake.math.RigidTransform_[AutoDiffXd]) numpy.ndarray[object[3, 1]]
Returns
p_AoBo_A
, the position vector portion ofthis
RigidTransform, i.e., position vector from Ao (frame A’s origin) to Bo (frame B’s origin).
- class pydrake.math.RigidTransform_[Expression]
This class represents a proper rigid transform between two frames which can be regarded in two ways. A rigid transform describes the “pose” between two frames A and B (i.e., the relative orientation and position of A to B). Alternately, it can be regarded as a 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 isR_AB
. The monogram notation for the position vector from Ao to Bo isp_AoBo_A
. See multibody_quantities for monogram notation for dynamics.Note
This class does not store the frames associated with the transform and cannot enforce correct usage of this class. For example, it makes sense to multiply RigidTransforms as
X_AB * X_BC
, but notX_AB * X_CB
.Note
This class is not a 4x4 transformation matrix – even though its operator*() methods act mostly like 4x4 matrix multiplication. Instead, this class contains a 3x3 rotation matrix class and a 3x1 position vector. To convert this to a 3x4 matrix, use GetAsMatrix34(). To convert this to a 4x4 matrix, use GetAsMatrix4(). To convert this to an Eigen::Isometry, use GetAsIsometry().
Note
An isometry is sometimes regarded as synonymous with rigid transform. The RigidTransform class has important advantages over Eigen::Isometry. - RigidTransform is built on an underlying rigorous 3x3 RotationMatrix class that has significant functionality for 3D orientation. - In Debug builds, RigidTransform requires a valid 3x3 rotation matrix and a valid (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).
- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.math.RigidTransform_[Expression]) -> None
Constructs the RigidTransform that corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence, the constructed RigidTransform contains an identity RotationMatrix and a zero position vector.
__init__(self: pydrake.math.RigidTransform_[Expression], other: pydrake.math.RigidTransform_[Expression]) -> None
__init__(self: pydrake.math.RigidTransform_[Expression], R: drake::math::RotationMatrix<drake::symbolic::Expression>, p: numpy.ndarray[object[3, 1]]) -> None
Constructs a RigidTransform from a rotation matrix and a position vector.
- Parameter
R
: rotation matrix relating frames A and B (e.g.,
R_AB
).- Parameter
p
: position vector from frame A’s origin to frame B’s origin, expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
__init__(self: pydrake.math.RigidTransform_[Expression], rpy: drake::math::RollPitchYaw<drake::symbolic::Expression>, p: numpy.ndarray[object[3, 1]]) -> None
Constructs a RigidTransform from a RollPitchYaw and a position vector.
- Parameter
rpy
: a RollPitchYaw which is a 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]
.
See also
RotationMatrix::RotationMatrix(const RollPitchYaw<T>&)
- Parameter
p
: position vector from frame A’s origin to frame B’s origin, expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
__init__(self: pydrake.math.RigidTransform_[Expression], quaternion: pydrake.common.eigen_geometry.Quaternion_[Expression], p: numpy.ndarray[object[3, 1]]) -> None
Constructs a RigidTransform from a Quaternion and a position vector.
- Parameter
quaternion
: a 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. –
See also
RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&)
__init__(self: pydrake.math.RigidTransform_[Expression], theta_lambda: pydrake.common.eigen_geometry.AngleAxis_[Expression], p: numpy.ndarray[object[3, 1]]) -> None
Constructs a RigidTransform from an AngleAxis and a position vector.
- Parameter
theta_lambda
: an Eigen::AngleAxis whose associated axis (vector direction herein called
lambda
) is 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. –
See also
RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&)
__init__(self: pydrake.math.RigidTransform_[Expression], R: drake::math::RotationMatrix<drake::symbolic::Expression>) -> None
Constructs a RigidTransform with a given RotationMatrix and a zero position vector.
- Parameter
R
: rotation matrix relating frames A and B (e.g.,
R_AB
).
__init__(self: pydrake.math.RigidTransform_[Expression], p: numpy.ndarray[object[3, 1]]) -> None
Constructs a RigidTransform that contains an identity RotationMatrix and a given position vector
p
.- Parameter
p
: position vector from frame A’s origin to frame B’s origin, expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
__init__(self: pydrake.math.RigidTransform_[Expression], pose: pydrake.common.eigen_geometry.Isometry3_[Expression]) -> None
Constructs a RigidTransform from an Eigen Isometry3.
- Parameter
pose
: Isometry3 that contains an allegedly valid rotation matrix
R_AB
and also contains a position vectorp_AoBo_A
from frame A’s origin to frame B’s origin.p_AoBo_A
must be expressed in frame A.
- Raises
RuntimeError in debug builds if R_AB is not a proper orthonormal –
3x3 rotation matrix. –
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix().__init__(self: pydrake.math.RigidTransform_[Expression], pose: numpy.ndarray[object[m, n]]) -> None
Constructs a RigidTransform from an appropriate Eigen expression.
- Parameter
pose
: Generic Eigen matrix expression.
- Raises
RuntimeError if the Eigen expression in pose does not resolve –
to a Vector3 or 3x4 matrix or 4x4 matrix or if the rotational part –
of pose is not a proper orthonormal 3x3 rotation matrix or if –
pose` is a 4x4 matrix whose final row is not [0, 0, 0, 1] –
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix().Note
This constructor prevents ambiguity that would otherwise exist for a RigidTransform constructor whose argument is an Eigen expression.
Click to expand C++ code...
const Vector3<double> position(4, 5, 6); const RigidTransform<double> X1(3 * position); ---------------------------------------------- const RotationMatrix<double> R(RollPitchYaw<double>(1, 2, 3)); Eigen::Matrix<double, 3, 4> pose34; pose34 << R.matrix(), position; const RigidTransform<double> X2(1.0 * pose34); ---------------------------------------------- Eigen::Matrix<double, 4, 4> pose4; pose4 << R.matrix(), position, 0, 0, 0, 1; const RigidTransform<double> X3(pose4 * pose4);
- template cast
Instantiations:
cast[Expression]
- cast[Expression](self: pydrake.math.RigidTransform_[Expression]) pydrake.math.RigidTransform_[Expression]
Creates a RigidTransform templatized on a scalar type U from a RigidTransform templatized on scalar type T. For example,
Click to expand C++ code...
RigidTransform<double> source = RigidTransform<double>::Identity(); RigidTransform<AutoDiffXd> foo = source.cast<AutoDiffXd>();
- Template parameter
U
: Scalar type on which the returned RigidTransform is templated.
Note
RigidTransform<From>::cast<To>()
creates a newRigidTransform<To>
from aRigidTransform<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s objects that underlie this RigidTransform. For example, Eigen currently allows cast from type double to AutoDiffXd, but not vice-versa.- Template parameter
- GetAsIsometry3(self: pydrake.math.RigidTransform_[Expression]) pydrake.common.eigen_geometry.Isometry3_[Expression]
Returns the isometry in ℜ³ that is equivalent to a RigidTransform.
- GetAsMatrix34(self: pydrake.math.RigidTransform_[Expression]) numpy.ndarray[object[3, 4]]
Returns the 3x4 matrix associated with this RigidTransform, i.e., X_AB.
Click to expand C++ code...
┌ ┐ │ 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.
Click to expand C++ code...
┌ ┐ │ R_AB p_AoBo_A │ │ │ │ 0 1 │ └ ┘
- GetMaximumAbsoluteDifference(self: pydrake.math.RigidTransform_[Expression], other: pydrake.math.RigidTransform_[Expression]) pydrake.symbolic.Expression
Computes the infinity norm of
this
- other (i.e., the maximum absolute value of the difference between the elements ofthis
andother
).- Parameter
other
: RigidTransform to subtract from
this
.
- Returns
‖this - other‖∞
- Parameter
- GetMaximumAbsoluteTranslationDifference(self: pydrake.math.RigidTransform_[Expression], other: pydrake.math.RigidTransform_[Expression]) pydrake.symbolic.Expression
Returns the maximum absolute value of the difference in the position vectors (translation) in
this
andother
. In other words, returns the infinity norm of the difference in the position vectors.- Parameter
other
: RigidTransform whose position vector is subtracted from the position vector in
this
.
- Parameter
- static Identity() pydrake.math.RigidTransform_[Expression]
Returns the identity RigidTransform (corresponds to coincident frames).
- Returns
the RigidTransform that corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence, the returned RigidTransform contains a 3x3 identity matrix and a zero position vector.
- inverse(self: pydrake.math.RigidTransform_[Expression]) pydrake.math.RigidTransform_[Expression]
Returns X_BA = X_AB⁻¹, the inverse of
this
RigidTransform.Note
The inverse of RigidTransform X_AB is X_BA, which contains the rotation matrix R_BA = R_AB⁻¹ = R_ABᵀ and the position vector
p_BoAo_B_
(position from B’s origin Bo to A’s origin Ao, expressed in frame B).Note
: The 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.
- InvertAndCompose(self: pydrake.math.RigidTransform_[Expression], other: pydrake.math.RigidTransform_[Expression]) pydrake.math.RigidTransform_[Expression]
Calculates the product of
this
inverted and another RigidTransform. If you considerthis
to be the transform X_AB, andother
to be X_AC, then this method returns X_BC = X_AB⁻¹ * X_AC. For T==double, this method can be much faster than inverting first and then performing the composition, because it can take advantage of the special structure of a rigid transform to avoid unnecessary memory and floating point operations. On some platforms it can use SIMD instructions for further speedups.- Parameter
other
: RigidTransform that post-multiplies
this
inverted.- Returns
X_BC
: where X_BC = this⁻¹ * other.
Note
It is possible (albeit improbable) to create an invalid rigid transform by accumulating round-off error with a large number of multiplies.
- Parameter
- IsExactlyEqualTo(self: pydrake.math.RigidTransform_[Expression], other: pydrake.math.RigidTransform_[Expression]) pydrake.symbolic.Formula
Returns true if
this
is exactly equal toother
.- Parameter
other
: RigidTransform to compare to
this
.
- Returns
True
if each element ofthis
is exactly equal to the corresponding element ofother
.
- Parameter
- IsExactlyIdentity(self: pydrake.math.RigidTransform_[Expression]) pydrake.symbolic.Formula
Returns
True
ifthis
is exactly the identity RigidTransform.See also
IsNearlyIdentity().
- IsNearlyEqualTo(self: pydrake.math.RigidTransform_[Expression], other: pydrake.math.RigidTransform_[Expression], tolerance: float) pydrake.symbolic.Formula
Compares each element of
this
to the corresponding element ofother
to check if they are the same to within a specifiedtolerance
.- Parameter
other
: RigidTransform to compare to
this
.- Parameter
tolerance
: maximum allowable absolute difference between the elements in
this
andother
.
- Returns
True
if‖this.matrix() - other.matrix()‖∞ <= tolerance
.
Note
Consider scaling tolerance with the largest of magA and magB, where magA and magB denoted the magnitudes of
this
position vector andother
position vectors, respectively.- Parameter
- IsNearlyIdentity(self: pydrake.math.RigidTransform_[Expression], translation_tolerance: float) pydrake.symbolic.Formula
Returns true if
this
is within tolerance of the identity RigidTransform.- Parameter
translation_tolerance
: a non-negative number. One way to choose
translation_tolerance
is to multiply a characteristic length (e.g., the magnitude of a characteristic position vector) by an epsilon (e.g., RotationMatrix::get_internal_tolerance_for_orthonormality()).
- Returns
True
if the RotationMatrix portion ofthis
satisfies RotationMatrix::IsNearlyIdentity() and if the position vector portion ofthis
is equal to zero vector withintranslation_tolerance
.
See also
IsExactlyIdentity().
- Parameter
- multiply(*args, **kwargs)
Overloaded function.
multiply(self: pydrake.math.RigidTransform_[Expression], other: pydrake.math.RigidTransform_[Expression]) -> pydrake.math.RigidTransform_[Expression]
Multiplies
this
RigidTransformX_AB
by theother
RigidTransformX_BC
and returns the RigidTransformX_AC = X_AB * X_BC
.multiply(self: pydrake.math.RigidTransform_[Expression], p_BoQ_B: numpy.ndarray[object[3, 1]]) -> numpy.ndarray[object[3, 1]]
Multiplies
this
RigidTransformX_AB
by the position vectorp_BoQ_B
which is from Bo (B’s origin) to an arbitrary point Q.- Parameter
p_BoQ_B
: position vector from Bo to Q, expressed in frame B.
- Returns
p_AoQ_A
: position vector from Ao to Q, expressed in frame A.
multiply(self: pydrake.math.RigidTransform_[Expression], vec_B: numpy.ndarray[object[4, 1]]) -> numpy.ndarray[object[4, 1]]
Multiplies
this
RigidTransformX_AB
by the 4-element vectorvec_B
, equivalent toX_AB.GetAsMatrix4() * vec_B
.- Parameter
vec_B
: 4-element vector whose first 3 elements are the position vector p_BoQ_B from Bo (frame B’s origin) to an arbitrary point Q, expressed in frame B and whose 4ᵗʰ element is 1 𝐨𝐫 whose first 3 elements are a vector (maybe unrelated to Bo or Q) and whose 4ᵗʰ element is 0.
- Returns
vec_A
: 4-element vector whose first 3 elements are the position vector p_AoQ_A from Ao (frame A’s origin) to Q, expressed in frame A and whose 4ᵗʰ element is 1 𝐨𝐫 whose first 3 elements are a vector (maybe unrelated to Bo and Q) and whose 4ᵗʰ element is 0.
- Raises
RuntimeError if the 4ᵗʰ element of vec_B is not 0 or 1. –
multiply(self: pydrake.math.RigidTransform_[Expression], p_BoQ_B: numpy.ndarray[object[3, n]]) -> numpy.ndarray[object[3, n]]
Multiplies
this
RigidTransformX_AB
by the n position vectorsp_BoQ1_B
… p_BoQn_B, wherep_BoQi_B
is the iᵗʰ position vector from Bo (frame B’s origin) to an arbitrary point Qi, expressed in frame B.- Parameter
p_BoQ_B
: 3 x n
matrix with n position vectorsp_BoQi_B
or an expression that resolves to a3 x n
matrix of position vectors.- Returns
p_AoQ_A
: 3 x n
matrix with n position vectorsp_AoQi_A
, i.e., n position vectors from Ao (frame A’s origin) to Qi, expressed in frame A. Specifically, this operator* is defined so thatX_AB * p_BoQ_B
returnsp_AoQ_A = p_AoBo_A + R_AB * p_BoQ_B
, wherep_AoBo_A
is the position vector from Ao to Bo expressed in A andR_AB
is the rotation matrix relating the orientation of frames A and B.
Note
As needed, use parentheses. This operator* is not associative. To see this, let
p = p_AoBo_A
, q = p_BoQ_B and note (X_AB * q) * 7 = (p + R_AB * q) * 7 ≠ X_AB * (q * 7) = p + R_AB * (q * 7).Click to expand C++ code...
const RollPitchYaw<double> rpy(0.1, 0.2, 0.3); const RigidTransform<double> X_AB(rpy, Vector3d(1, 2, 3)); Eigen::Matrix<double, 3, 2> p_BoQ_B; p_BoQ_B.col(0) = Vector3d(4, 5, 6); p_BoQ_B.col(1) = Vector3d(9, 8, 7); const Eigen::Matrix<double, 3, 2> p_AoQ_A = X_AB * p_BoQ_B;
- rotation(self: pydrake.math.RigidTransform_[Expression]) drake::math::RotationMatrix<drake::symbolic::Expression>
Returns R_AB, the rotation matrix portion of
this
RigidTransform.- Returns
R_AB
: the rotation matrix portion of
this
RigidTransform.
- Returns
- set(self: pydrake.math.RigidTransform_[Expression], R: drake::math::RotationMatrix<drake::symbolic::Expression>, p: numpy.ndarray[object[3, 1]]) None
Sets
this
RigidTransform from a RotationMatrix and a position vector.- Parameter
R
: rotation matrix relating frames A and B (e.g.,
R_AB
).- Parameter
p
: position vector from frame A’s origin to frame B’s origin, expressed in frame A. In monogram notation p is denoted
p_AoBo_A
.
- Parameter
- set_rotation(*args, **kwargs)
Overloaded function.
set_rotation(self: pydrake.math.RigidTransform_[Expression], R: drake::math::RotationMatrix<drake::symbolic::Expression>) -> None
Sets the RotationMatrix portion of
this
RigidTransform.- Parameter
R
: rotation matrix relating frames A and B (e.g.,
R_AB
).
set_rotation(self: pydrake.math.RigidTransform_[Expression], rpy: drake::math::RollPitchYaw<drake::symbolic::Expression>) -> None
Sets the rotation part of
this
RigidTransform from a RollPitchYaw.- Parameter
rpy
: “roll-pitch-yaw” angles.
See also
RotationMatrix::RotationMatrix(const RollPitchYaw<T>&) which describes the parameter, preconditions, etc.
set_rotation(self: pydrake.math.RigidTransform_[Expression], quaternion: pydrake.common.eigen_geometry.Quaternion_[Expression]) -> None
Sets the rotation part of
this
RigidTransform from a Quaternion.- Parameter
quaternion
: a quaternion which may or may not have unit length.
See also
RotationMatrix::RotationMatrix(const Eigen::Quaternion<T>&) which describes the parameter, preconditions, exception conditions, etc.
set_rotation(self: pydrake.math.RigidTransform_[Expression], theta_lambda: pydrake.common.eigen_geometry.AngleAxis_[Expression]) -> None
Sets the rotation part of
this
RigidTransform from an AngleAxis.- Parameter
theta_lambda
: an angle
theta
(in radians) and vectorlambda
.
See also
RotationMatrix::RotationMatrix(const Eigen::AngleAxis<T>&) which describes the parameter, preconditions, exception conditions, etc.
- set_translation(self: pydrake.math.RigidTransform_[Expression], p: numpy.ndarray[object[3, 1]]) None
Sets the position vector portion of
this
RigidTransform.- Parameter
p
: position vector from Ao (frame A’s origin) to Bo (frame B’s origin) expressed in frame A. In monogram notation p is denoted p_AoBo_A.
- Parameter
- SetFromIsometry3(self: pydrake.math.RigidTransform_[Expression], pose: pydrake.common.eigen_geometry.Isometry3_[Expression]) None
Sets
this
RigidTransform from an Eigen Isometry3.- Parameter
pose
: Isometry3 that contains an allegedly valid rotation matrix
R_AB
and also contains a position vectorp_AoBo_A
from frame A’s origin to frame B’s origin.p_AoBo_A
must be expressed in frame A.
- Raises
RuntimeError in debug builds if R_AB is not a proper orthonormal –
3x3 rotation matrix. –
Note
No attempt is made to orthogonalize the 3x3 rotation matrix part of
pose
. As needed, use RotationMatrix::ProjectToRotationMatrix().- Parameter
- SetIdentity(self: pydrake.math.RigidTransform_[Expression]) pydrake.math.RigidTransform_[Expression]
Sets
this
RigidTransform so it corresponds to aligning the two frames so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. Hence,this
RigidTransform contains a 3x3 identity matrix and a zero position vector.
- translation(self: pydrake.math.RigidTransform_[Expression]) numpy.ndarray[object[3, 1]]
Returns
p_AoBo_A
, the position vector portion ofthis
RigidTransform, i.e., position vector from Ao (frame A’s origin) to Bo (frame B’s origin).
- class pydrake.math.RollPitchYaw
This class represents the orientation between two arbitrary frames A and D associated with a Space-fixed (extrinsic) X-Y-Z rotation by “roll-pitch-yaw” angles
[r, p, y]
, which is equivalent to a Body-fixed (intrinsic) Z-Y-X rotation by “yaw-pitch-roll” angles[y, p, r]
. The rotation matrixR_AD
associated with this roll-pitch-yaw[r, p, y]
rotation sequence is equal to the matrix multiplication shown below.Click to expand C++ code...
⎡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
aboutDx = Cx
. Note: D and C are no longer aligned.2nd rotation R_BC: Frames D, C (collectively – as if welded together)
rotate relative to frame B, A by a pitch angle
p
aboutCy = By
. Note: C and B are no longer aligned.3rd rotation R_AB: Frames D, C, B (collectively – as if welded)
rotate relative to frame A by a yaw angle
y
aboutBz = Az
. Note: B and A are no longer aligned. The monogram notation for the rotation matrix relating A to D isR_AD
.See also
multibody_quantities for monogram notation for dynamics and orientation_discussion “a discussion on rotation matrices”.
Note
This class does not store the frames associated with this rotation sequence.
Note
This class is templated; see
RollPitchYaw_
for the list of instantiations.- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.math.RollPitchYaw, other: pydrake.math.RollPitchYaw) -> None
__init__(self: pydrake.math.RollPitchYaw, rpy: numpy.ndarray[numpy.float64[3, 1]]) -> None
Constructs a RollPitchYaw from a 3x1 array of angles.
- Parameter
rpy
: 3x1 array with roll, pitch, yaw angles (units of radians).
- Raises
RuntimeError in debug builds if !IsValid(rpy) –
__init__(self: pydrake.math.RollPitchYaw, 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<T>(roll, pitch, –
yaw)) –
__init__(self: pydrake.math.RollPitchYaw, R: pydrake.math.RotationMatrix) -> 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.
__init__(self: pydrake.math.RollPitchYaw, quaternion: pydrake.common.eigen_geometry.Quaternion) -> 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) –
__init__(self: pydrake.math.RollPitchYaw, matrix: numpy.ndarray[numpy.float64[3, 3]]) -> None
Construct from raw rotation matrix. See RotationMatrix overload for more information.
- CalcAngularVelocityInChildFromRpyDt(self: pydrake.math.RollPitchYaw, rpyDt: numpy.ndarray[numpy.float64[3, 1]]) numpy.ndarray[numpy.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.,[ṙ; ṗ; ẏ]
.
- Returns
w_AD_D, frame D’s angular velocity in frame A, expressed in “child” frame D. In other words, returns [ω0; ω1; ω2]ᴅ, where
w_AD_D = ω0 Dx + ω1 Dy + ω2 Dz
, and where [ω0; ω1; ω2]ᴅ is calculated via the 3x3 matrix Nc⁻¹ (the inverse of the matrix Nc documented in CalcMatrixRelatingRpyDtToAngularVelocityInChild()).
Click to expand C++ code...
⌈ ω0 ⌉ ⌈ ṙ ⌉ ⌈ 1 0 -sin(p) ⌉ | ω1 | = Nc⁻¹ | ṗ | Nc⁻¹ = | 0 cos(r) sin(r)*cos(p) | ⌊ ω2 ⌋ᴅ ⌊ ẏ ⌋ ⌊ 0 -sin(r) cos(r)*cos(p) ⌋
- Parameter
- CalcAngularVelocityInParentFromRpyDt(self: pydrake.math.RollPitchYaw, rpyDt: numpy.ndarray[numpy.float64[3, 1]]) numpy.ndarray[numpy.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.,[ṙ; ṗ; ẏ]
.
- Returns
w_AD_A, frame D’s angular velocity in frame A, expressed in “parent” frame A. In other words, returns [ωx; ωy; ωz]ᴀ, where
w_AD_A = ωx Ax + ωy Ay + ωz Az
, and where [ωx; ωy; ωz]ᴀ is calculated via the the 3x3 matrix Np⁻¹ (the inverse of the matrix Np documented in CalcMatrixRelatingRpyDtToAngularVelocityInParent()).
Click to expand C++ code...
⌈ ωx ⌉ ⌈ ṙ ⌉ ⌈ cos(y)*cos(p) -sin(y) 0 ⌉ | ωy | = Np⁻¹ | ṗ | Np⁻¹ = | sin(y)*cos(p) cos(y) 0 | ⌊ ωz ⌋ᴀ ⌊ ẏ ⌋ ⌊ -sin(p) 0 1 ⌋
- Parameter
- CalcRotationMatrixDt(self: pydrake.math.RollPitchYaw, rpyDt: numpy.ndarray[numpy.float64[3, 1]]) numpy.ndarray[numpy.float64[3, 3]]
Forms Ṙ, the ordinary derivative of the RotationMatrix
R
with respect to an independent variablet
(t usually denotes time) andR
is the RotationMatrix formed bythis
RollPitchYaw. The roll-pitch-yaw angles r, p, y are regarded as functions oft
[i.e., r(t), p(t), y(t)].- Parameter
rpyDt
: Ordinary derivative of rpy with respect to an independent variable
t
(t usually denotes time, but not necessarily).
- Returns
Ṙ, the ordinary derivative of
R
with respect tot
, calculated as Ṙ = ∂R/∂r * ṙ + ∂R/∂p * ṗ + ∂R/∂y * ẏ. In other words, the returned (i, j) element is ∂Rij/∂r * ṙ + ∂Rij/∂p * ṗ + ∂Rij/∂y * ẏ.
- Parameter
- CalcRpyDDtFromAngularAccelInChild(self: pydrake.math.RollPitchYaw, rpyDt: numpy.ndarray[numpy.float64[3, 1]], alpha_AD_D: numpy.ndarray[numpy.float64[3, 1]]) numpy.ndarray[numpy.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.,[ṙ; ṗ; ẏ]
.- Parameter
alpha_AD_D
: , frame D’s angular acceleration in frame A, expressed in frame D.
- Returns
[r̈, p̈, ÿ]
, the 2ⁿᵈ time-derivative ofthis
RollPitchYaw.- Raises
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., whencos(p) ≈ 0
.- Parameter
- CalcRpyDDtFromRpyDtAndAngularAccelInParent(self: pydrake.math.RollPitchYaw, rpyDt: numpy.ndarray[numpy.float64[3, 1]], alpha_AD_A: numpy.ndarray[numpy.float64[3, 1]]) numpy.ndarray[numpy.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.,[ṙ; ṗ; ẏ]
.- Parameter
alpha_AD_A
: , frame D’s angular acceleration in frame A, expressed in frame A.
- Returns
[r̈, p̈, ÿ]
, the 2ⁿᵈ time-derivative ofthis
RollPitchYaw.- Raises
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., whencos(p) ≈ 0
.- Parameter
- CalcRpyDtFromAngularVelocityInChild(self: pydrake.math.RollPitchYaw, w_AD_D: numpy.ndarray[numpy.float64[3, 1]]) numpy.ndarray[numpy.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.- Parameter
w_AD_D
: , frame D’s angular velocity in frame A, expressed in D.
- Returns
[ṙ; ṗ; ẏ]
, the 1ˢᵗ time-derivative ofthis
RollPitchYaw.- Raises
RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock) –
Note
Enhanced documentation for this method and its gimbal-lock (divide- by-zero error) is in CalcMatrixRelatingRpyDtToAngularVelocityInChild().
See also
CalcRpyDtFromAngularVelocityInParent()
- Parameter
- CalcRpyDtFromAngularVelocityInParent(self: pydrake.math.RollPitchYaw, w_AD_A: numpy.ndarray[numpy.float64[3, 1]]) numpy.ndarray[numpy.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.- Parameter
w_AD_A
: , frame D’s angular velocity in frame A, expressed in A.
- Returns
[ṙ; ṗ; ẏ]
, the 1ˢᵗ time-derivative ofthis
RollPitchYaw.- Raises
RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock) –
Note
Enhanced documentation for this method and its gimbal-lock (divide- by-zero error) is in CalcMatrixRelatingRpyDtToAngularVelocityInParent().
See also
CalcRpyDtFromAngularVelocityInChild()
- Parameter
- pitch_angle(self: pydrake.math.RollPitchYaw) float
Returns the pitch-angle underlying
this
RollPitchYaw.
- roll_angle(self: pydrake.math.RollPitchYaw) float
Returns the roll-angle underlying
this
RollPitchYaw.
- ToQuaternion(self: pydrake.math.RollPitchYaw) pydrake.common.eigen_geometry.Quaternion
Returns a quaternion representation of
this
RollPitchYaw.
- ToRotationMatrix(self: pydrake.math.RollPitchYaw) pydrake.math.RotationMatrix
Returns the RotationMatrix representation of
this
RollPitchYaw.
- vector(self: pydrake.math.RollPitchYaw) numpy.ndarray[numpy.float64[3, 1]]
Returns the Vector3 underlying
this
RollPitchYaw.
- yaw_angle(self: pydrake.math.RollPitchYaw) float
Returns the yaw-angle underlying
this
RollPitchYaw.
- template pydrake.math.RollPitchYaw_
Instantiations:
RollPitchYaw_[float]
,RollPitchYaw_[AutoDiffXd]
,RollPitchYaw_[Expression]
- 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 matrixR_AD
associated with this roll-pitch-yaw[r, p, y]
rotation sequence is equal to the matrix multiplication shown below.Click to expand C++ code...
⎡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
aboutDx = Cx
. Note: D and C are no longer aligned.2nd rotation R_BC: Frames D, C (collectively – as if welded together)
rotate relative to frame B, A by a pitch angle
p
aboutCy = By
. Note: C and B are no longer aligned.3rd rotation R_AB: Frames D, C, B (collectively – as if welded)
rotate relative to frame A by a yaw angle
y
aboutBz = Az
. Note: B and A are no longer aligned. The monogram notation for the rotation matrix relating A to D isR_AD
.See also
multibody_quantities for monogram notation for dynamics and orientation_discussion “a discussion on rotation matrices”.
Note
This class does not store the frames associated with this rotation sequence.
- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.math.RollPitchYaw_[AutoDiffXd], other: pydrake.math.RollPitchYaw_[AutoDiffXd]) -> None
__init__(self: pydrake.math.RollPitchYaw_[AutoDiffXd], rpy: numpy.ndarray[object[3, 1]]) -> None
Constructs a RollPitchYaw from a 3x1 array of angles.
- Parameter
rpy
: 3x1 array with roll, pitch, yaw angles (units of radians).
- Raises
RuntimeError in debug builds if !IsValid(rpy) –
__init__(self: pydrake.math.RollPitchYaw_[AutoDiffXd], roll: pydrake.autodiffutils.AutoDiffXd, pitch: pydrake.autodiffutils.AutoDiffXd, yaw: pydrake.autodiffutils.AutoDiffXd) -> None
Constructs a RollPitchYaw from roll, pitch, yaw angles (radian units).
- Parameter
roll
: 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<T>(roll, pitch, –
yaw)) –
__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.
__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) –
__init__(self: pydrake.math.RollPitchYaw_[AutoDiffXd], matrix: numpy.ndarray[object[3, 3]]) -> None
Construct from raw rotation matrix. See RotationMatrix overload for more information.
- CalcAngularVelocityInChildFromRpyDt(self: pydrake.math.RollPitchYaw_[AutoDiffXd], rpyDt: numpy.ndarray[object[3, 1]]) numpy.ndarray[object[3, 1]]
Calculates angular velocity from
this
RollPitchYaw whose 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.,[ṙ; ṗ; ẏ]
.
- Returns
w_AD_D, frame D’s angular velocity in frame A, expressed in “child” frame D. In other words, returns [ω0; ω1; ω2]ᴅ, where
w_AD_D = ω0 Dx + ω1 Dy + ω2 Dz
, and where [ω0; ω1; ω2]ᴅ is calculated via the 3x3 matrix Nc⁻¹ (the inverse of the matrix Nc documented in CalcMatrixRelatingRpyDtToAngularVelocityInChild()).
Click to expand C++ code...
⌈ ω0 ⌉ ⌈ ṙ ⌉ ⌈ 1 0 -sin(p) ⌉ | ω1 | = Nc⁻¹ | ṗ | Nc⁻¹ = | 0 cos(r) sin(r)*cos(p) | ⌊ ω2 ⌋ᴅ ⌊ ẏ ⌋ ⌊ 0 -sin(r) cos(r)*cos(p) ⌋
- Parameter
- CalcAngularVelocityInParentFromRpyDt(self: pydrake.math.RollPitchYaw_[AutoDiffXd], rpyDt: numpy.ndarray[object[3, 1]]) numpy.ndarray[object[3, 1]]
Calculates angular velocity from
this
RollPitchYaw whose 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.,[ṙ; ṗ; ẏ]
.
- Returns
w_AD_A, frame D’s angular velocity in frame A, expressed in “parent” frame A. In other words, returns [ωx; ωy; ωz]ᴀ, where
w_AD_A = ωx Ax + ωy Ay + ωz Az
, and where [ωx; ωy; ωz]ᴀ is calculated via the the 3x3 matrix Np⁻¹ (the inverse of the matrix Np documented in CalcMatrixRelatingRpyDtToAngularVelocityInParent()).
Click to expand C++ code...
⌈ ωx ⌉ ⌈ ṙ ⌉ ⌈ cos(y)*cos(p) -sin(y) 0 ⌉ | ωy | = Np⁻¹ | ṗ | Np⁻¹ = | sin(y)*cos(p) cos(y) 0 | ⌊ ωz ⌋ᴀ ⌊ ẏ ⌋ ⌊ -sin(p) 0 1 ⌋
- Parameter
- CalcRotationMatrixDt(self: pydrake.math.RollPitchYaw_[AutoDiffXd], rpyDt: numpy.ndarray[object[3, 1]]) numpy.ndarray[object[3, 3]]
Forms Ṙ, the ordinary derivative of the RotationMatrix
R
with respect to an independent variablet
(t usually denotes time) andR
is the RotationMatrix formed bythis
RollPitchYaw. The roll-pitch-yaw angles r, p, y are regarded as functions oft
[i.e., r(t), p(t), y(t)].- Parameter
rpyDt
: Ordinary derivative of rpy with respect to an independent variable
t
(t usually denotes time, but not necessarily).
- Returns
Ṙ, the ordinary derivative of
R
with respect tot
, calculated as Ṙ = ∂R/∂r * ṙ + ∂R/∂p * ṗ + ∂R/∂y * ẏ. In other words, the returned (i, j) element is ∂Rij/∂r * ṙ + ∂Rij/∂p * ṗ + ∂Rij/∂y * ẏ.
- Parameter
- CalcRpyDDtFromAngularAccelInChild(self: pydrake.math.RollPitchYaw_[AutoDiffXd], rpyDt: numpy.ndarray[object[3, 1]], alpha_AD_D: numpy.ndarray[object[3, 1]]) numpy.ndarray[object[3, 1]]
Uses angular acceleration to compute the 2ⁿᵈ 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.,[ṙ; ṗ; ẏ]
.- Parameter
alpha_AD_D
: , frame D’s angular acceleration in frame A, expressed in frame D.
- Returns
[r̈, p̈, ÿ]
, the 2ⁿᵈ time-derivative ofthis
RollPitchYaw.- Raises
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., whencos(p) ≈ 0
.- Parameter
- CalcRpyDDtFromRpyDtAndAngularAccelInParent(self: pydrake.math.RollPitchYaw_[AutoDiffXd], rpyDt: numpy.ndarray[object[3, 1]], alpha_AD_A: numpy.ndarray[object[3, 1]]) numpy.ndarray[object[3, 1]]
Uses angular acceleration to compute the 2ⁿᵈ 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.,[ṙ; ṗ; ẏ]
.- Parameter
alpha_AD_A
: , frame D’s angular acceleration in frame A, expressed in frame A.
- Returns
[r̈, p̈, ÿ]
, the 2ⁿᵈ time-derivative ofthis
RollPitchYaw.- Raises
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., whencos(p) ≈ 0
.- Parameter
- CalcRpyDtFromAngularVelocityInChild(self: pydrake.math.RollPitchYaw_[AutoDiffXd], w_AD_D: 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.- Parameter
w_AD_D
: , frame D’s angular velocity in frame A, expressed in D.
- Returns
[ṙ; ṗ; ẏ]
, the 1ˢᵗ time-derivative ofthis
RollPitchYaw.- Raises
RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock) –
Note
Enhanced documentation for this method and its gimbal-lock (divide- by-zero error) is in CalcMatrixRelatingRpyDtToAngularVelocityInChild().
See also
CalcRpyDtFromAngularVelocityInParent()
- Parameter
- CalcRpyDtFromAngularVelocityInParent(self: pydrake.math.RollPitchYaw_[AutoDiffXd], w_AD_A: numpy.ndarray[object[3, 1]]) numpy.ndarray[object[3, 1]]
Uses angular velocity to compute the 1ˢᵗ time-derivative of
this
RollPitchYaw whose angles[r; p; y]
orient two generic frames A and D.- Parameter
w_AD_A
: , frame D’s angular velocity in frame A, expressed in A.
- Returns
[ṙ; ṗ; ẏ]
, the 1ˢᵗ time-derivative ofthis
RollPitchYaw.- Raises
RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock) –
Note
Enhanced documentation for this method and its gimbal-lock (divide- by-zero error) is in CalcMatrixRelatingRpyDtToAngularVelocityInParent().
See also
CalcRpyDtFromAngularVelocityInChild()
- Parameter
- 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.
- ToQuaternion(self: pydrake.math.RollPitchYaw_[AutoDiffXd]) pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd]
Returns a quaternion representation of
this
RollPitchYaw.
- ToRotationMatrix(self: pydrake.math.RollPitchYaw_[AutoDiffXd]) pydrake.math.RotationMatrix_[AutoDiffXd]
Returns the RotationMatrix representation of
this
RollPitchYaw.
- vector(self: pydrake.math.RollPitchYaw_[AutoDiffXd]) numpy.ndarray[object[3, 1]]
Returns the Vector3 underlying
this
RollPitchYaw.
- yaw_angle(self: pydrake.math.RollPitchYaw_[AutoDiffXd]) pydrake.autodiffutils.AutoDiffXd
Returns the 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 matrixR_AD
associated with this roll-pitch-yaw[r, p, y]
rotation sequence is equal to the matrix multiplication shown below.Click to expand C++ code...
⎡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
aboutDx = Cx
. Note: D and C are no longer aligned.2nd rotation R_BC: Frames D, C (collectively – as if welded together)
rotate relative to frame B, A by a pitch angle
p
aboutCy = By
. Note: C and B are no longer aligned.3rd rotation R_AB: Frames D, C, B (collectively – as if welded)
rotate relative to frame A by a yaw angle
y
aboutBz = Az
. Note: B and A are no longer aligned. The monogram notation for the rotation matrix relating A to D isR_AD
.See also
multibody_quantities for monogram notation for dynamics and orientation_discussion “a discussion on rotation matrices”.
Note
This class does not store the frames associated with this rotation sequence.
- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.math.RollPitchYaw_[Expression], other: pydrake.math.RollPitchYaw_[Expression]) -> None
__init__(self: pydrake.math.RollPitchYaw_[Expression], rpy: numpy.ndarray[object[3, 1]]) -> None
Constructs a RollPitchYaw from a 3x1 array of angles.
- Parameter
rpy
: 3x1 array with roll, pitch, yaw angles (units of radians).
- Raises
RuntimeError in debug builds if !IsValid(rpy) –
__init__(self: pydrake.math.RollPitchYaw_[Expression], roll: pydrake.symbolic.Expression, pitch: pydrake.symbolic.Expression, yaw: pydrake.symbolic.Expression) -> None
Constructs a RollPitchYaw from roll, pitch, yaw angles (radian units).
- Parameter
roll
: 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<T>(roll, pitch, –
yaw)) –
__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.
__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) –
__init__(self: pydrake.math.RollPitchYaw_[Expression], matrix: numpy.ndarray[object[3, 3]]) -> None
Construct from raw rotation matrix. See RotationMatrix overload for more information.
- CalcAngularVelocityInChildFromRpyDt(self: pydrake.math.RollPitchYaw_[Expression], rpyDt: numpy.ndarray[object[3, 1]]) numpy.ndarray[object[3, 1]]
Calculates angular velocity from
this
RollPitchYaw whose 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.,[ṙ; ṗ; ẏ]
.
- Returns
w_AD_D, frame D’s angular velocity in frame A, expressed in “child” frame D. In other words, returns [ω0; ω1; ω2]ᴅ, where
w_AD_D = ω0 Dx + ω1 Dy + ω2 Dz
, and where [ω0; ω1; ω2]ᴅ is calculated via the 3x3 matrix Nc⁻¹ (the inverse of the matrix Nc documented in CalcMatrixRelatingRpyDtToAngularVelocityInChild()).
Click to expand C++ code...
⌈ ω0 ⌉ ⌈ ṙ ⌉ ⌈ 1 0 -sin(p) ⌉ | ω1 | = Nc⁻¹ | ṗ | Nc⁻¹ = | 0 cos(r) sin(r)*cos(p) | ⌊ ω2 ⌋ᴅ ⌊ ẏ ⌋ ⌊ 0 -sin(r) cos(r)*cos(p) ⌋
- Parameter
- CalcAngularVelocityInParentFromRpyDt(self: pydrake.math.RollPitchYaw_[Expression], rpyDt: numpy.ndarray[object[3, 1]]) numpy.ndarray[object[3, 1]]
Calculates angular velocity from
this
RollPitchYaw whose 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.,[ṙ; ṗ; ẏ]
.
- Returns
w_AD_A, frame D’s angular velocity in frame A, expressed in “parent” frame A. In other words, returns [ωx; ωy; ωz]ᴀ, where
w_AD_A = ωx Ax + ωy Ay + ωz Az
, and where [ωx; ωy; ωz]ᴀ is calculated via the the 3x3 matrix Np⁻¹ (the inverse of the matrix Np documented in CalcMatrixRelatingRpyDtToAngularVelocityInParent()).
Click to expand C++ code...
⌈ ωx ⌉ ⌈ ṙ ⌉ ⌈ cos(y)*cos(p) -sin(y) 0 ⌉ | ωy | = Np⁻¹ | ṗ | Np⁻¹ = | sin(y)*cos(p) cos(y) 0 | ⌊ ωz ⌋ᴀ ⌊ ẏ ⌋ ⌊ -sin(p) 0 1 ⌋
- Parameter
- CalcRotationMatrixDt(self: pydrake.math.RollPitchYaw_[Expression], rpyDt: numpy.ndarray[object[3, 1]]) numpy.ndarray[object[3, 3]]
Forms Ṙ, the ordinary derivative of the RotationMatrix
R
with respect to an independent variablet
(t usually denotes time) andR
is the RotationMatrix formed bythis
RollPitchYaw. The roll-pitch-yaw angles r, p, y are regarded as functions oft
[i.e., r(t), p(t), y(t)].- Parameter
rpyDt
: Ordinary derivative of rpy with respect to an independent variable
t
(t usually denotes time, but not necessarily).
- Returns
Ṙ, the ordinary derivative of
R
with respect tot
, calculated as Ṙ = ∂R/∂r * ṙ + ∂R/∂p * ṗ + ∂R/∂y * ẏ. In other words, the returned (i, j) element is ∂Rij/∂r * ṙ + ∂Rij/∂p * ṗ + ∂Rij/∂y * ẏ.
- Parameter
- CalcRpyDDtFromAngularAccelInChild(self: pydrake.math.RollPitchYaw_[Expression], rpyDt: numpy.ndarray[object[3, 1]], alpha_AD_D: numpy.ndarray[object[3, 1]]) numpy.ndarray[object[3, 1]]
Uses angular acceleration to compute the 2ⁿᵈ 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.,[ṙ; ṗ; ẏ]
.- Parameter
alpha_AD_D
: , frame D’s angular acceleration in frame A, expressed in frame D.
- Returns
[r̈, p̈, ÿ]
, the 2ⁿᵈ time-derivative ofthis
RollPitchYaw.- Raises
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., whencos(p) ≈ 0
.- Parameter
- CalcRpyDDtFromRpyDtAndAngularAccelInParent(self: pydrake.math.RollPitchYaw_[Expression], rpyDt: numpy.ndarray[object[3, 1]], alpha_AD_A: numpy.ndarray[object[3, 1]]) numpy.ndarray[object[3, 1]]
Uses angular acceleration to compute the 2ⁿᵈ 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.,[ṙ; ṗ; ẏ]
.- Parameter
alpha_AD_A
: , frame D’s angular acceleration in frame A, expressed in frame A.
- Returns
[r̈, p̈, ÿ]
, the 2ⁿᵈ time-derivative ofthis
RollPitchYaw.- Raises
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., whencos(p) ≈ 0
.- Parameter
- CalcRpyDtFromAngularVelocityInChild(self: pydrake.math.RollPitchYaw_[Expression], w_AD_D: 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.- Parameter
w_AD_D
: , frame D’s angular velocity in frame A, expressed in D.
- Returns
[ṙ; ṗ; ẏ]
, the 1ˢᵗ time-derivative ofthis
RollPitchYaw.- Raises
RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock) –
Note
Enhanced documentation for this method and its gimbal-lock (divide- by-zero error) is in CalcMatrixRelatingRpyDtToAngularVelocityInChild().
See also
CalcRpyDtFromAngularVelocityInParent()
- Parameter
- CalcRpyDtFromAngularVelocityInParent(self: pydrake.math.RollPitchYaw_[Expression], w_AD_A: numpy.ndarray[object[3, 1]]) numpy.ndarray[object[3, 1]]
Uses angular velocity to compute the 1ˢᵗ time-derivative of
this
RollPitchYaw whose angles[r; p; y]
orient two generic frames A and D.- Parameter
w_AD_A
: , frame D’s angular velocity in frame A, expressed in A.
- Returns
[ṙ; ṗ; ẏ]
, the 1ˢᵗ time-derivative ofthis
RollPitchYaw.- Raises
RuntimeError if cos(p) ≈ 0 (p is near gimbal-lock) –
Note
Enhanced documentation for this method and its gimbal-lock (divide- by-zero error) is in CalcMatrixRelatingRpyDtToAngularVelocityInParent().
See also
CalcRpyDtFromAngularVelocityInChild()
- Parameter
- 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.
- ToQuaternion(self: pydrake.math.RollPitchYaw_[Expression]) pydrake.common.eigen_geometry.Quaternion_[Expression]
Returns a quaternion representation of
this
RollPitchYaw.
- ToRotationMatrix(self: pydrake.math.RollPitchYaw_[Expression]) pydrake.math.RotationMatrix_[Expression]
Returns the RotationMatrix representation of
this
RollPitchYaw.
- vector(self: pydrake.math.RollPitchYaw_[Expression]) numpy.ndarray[object[3, 1]]
Returns the Vector3 underlying
this
RollPitchYaw.
- yaw_angle(self: pydrake.math.RollPitchYaw_[Expression]) pydrake.symbolic.Expression
Returns the yaw-angle underlying
this
RollPitchYaw.
- class pydrake.math.RotationMatrix
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 isv_A = R_AB * v_B
, wherev_B
denotes an arbitrary vector v expressed in terms of Bx, By, Bz andv_A
denotes vector v expressed in terms of Ax, Ay, Az. See multibody_quantities for monogram notation for dynamics. See orientation_discussion “a discussion on rotation matrices”.Note
This class does not store the frames associated with a rotation matrix nor does it enforce strict proper usage of this class with vectors.
Note
When assertions are enabled, several methods in this class perform a validity check and throw RuntimeError if the rotation matrix is invalid. When assertions are disabled, many of these validity checks are skipped (which helps improve speed). 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).
Note
This class is templated; see
RotationMatrix_
for the list of instantiations.- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.math.RotationMatrix) -> None
Constructs a 3x3 identity RotationMatrix – which corresponds to aligning two frames (so that unit vectors Ax = Bx, Ay = By, Az = Bz).
__init__(self: pydrake.math.RotationMatrix, other: pydrake.math.RotationMatrix) -> None
__init__(self: pydrake.math.RotationMatrix, R: numpy.ndarray[numpy.float64[3, 3]]) -> None
Constructs a RotationMatrix from a Matrix3.
- Parameter
R
: an allegedly valid rotation matrix.
- Raises
RuntimeError in debug builds if R fails IsValid(R) –
__init__(self: pydrake.math.RotationMatrix, quaternion: pydrake.common.eigen_geometry.Quaternion) -> 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.__init__(self: pydrake.math.RotationMatrix, theta_lambda: pydrake.common.eigen_geometry.AngleAxis) -> 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. –
__init__(self: pydrake.math.RotationMatrix, 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]
.- Returns
R_AD
: , rotation matrix relating frame A to frame D.
Note
Denoting roll
r
, pitchp
, yawy
, this method returns a rotation matrixR_AD
equal to the matrix multiplication shown below.Click to expand C++ code...
⎡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
aboutDx = Cx
. Note: D and C are no longer aligned.2nd rotation R_BC: Frames D, C (collectively – as if welded together)
rotate relative to frame B, A by a pitch angle
p
aboutCy = By
. Note: C and B are no longer aligned.3rd rotation R_AB: Frames D, C, B (collectively – as if welded)
rotate relative to frame A by a roll angle
y
aboutBz = Az
. Note: B and A are no longer aligned.Note
This method constructs a RotationMatrix from a RollPitchYaw. Vice-versa, there are high-accuracy RollPitchYaw constructor/methods that form a RollPitchYaw from a rotation matrix.
- template cast
Instantiations:
cast[float]
,cast[AutoDiffXd]
,cast[Expression]
- cast[AutoDiffXd](self: pydrake.math.RotationMatrix) 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,
Click to expand C++ code...
RotationMatrix<double> source = RotationMatrix<double>::Identity(); RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
- Template parameter
U
: Scalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>()
creates a newRotationMatrix<To>
from aRotationMatrix<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not vice-versa.- Template parameter
- cast[Expression](self: pydrake.math.RotationMatrix) 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,
Click to expand C++ code...
RotationMatrix<double> source = RotationMatrix<double>::Identity(); RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
- Template parameter
U
: Scalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>()
creates a newRotationMatrix<To>
from aRotationMatrix<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not vice-versa.- Template parameter
- cast[float](self: pydrake.math.RotationMatrix) pydrake.math.RotationMatrix
Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T. For example,
Click to expand C++ code...
RotationMatrix<double> source = RotationMatrix<double>::Identity(); RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
- Template parameter
U
: Scalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>()
creates a newRotationMatrix<To>
from aRotationMatrix<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not vice-versa.- Template parameter
- col(self: pydrake.math.RotationMatrix, index: int) numpy.ndarray[numpy.float64[3, 1]]
Returns
this
rotation matrix’s iᵗʰ column (i = 0, 1, 2). Forthis
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).
See also
row(), matrix()
- Raises
In Debug builds, asserts (0 <= index <= 2) –
Note
For efficiency and consistency with Eigen, this method returns the same quantity returned by Eigen’s col() operator. The returned quantity can be assigned in various ways, e.g., as
const auto& Bz_A = col(2);
orVector3<T> Bz_A = col(2);
- Parameter
- static Identity() pydrake.math.RotationMatrix
- inverse(self: pydrake.math.RotationMatrix) pydrake.math.RotationMatrix
- InvertAndCompose(self: pydrake.math.RotationMatrix, other: pydrake.math.RotationMatrix) pydrake.math.RotationMatrix
Calculates the product of
this
inverted and another RotationMatrix. If you considerthis
to be the rotation matrix R_AB, andother
to be R_AC, then this method returns R_BC = R_AB⁻¹ * R_AC. For T==double, this method can be much faster than inverting first and then performing the composition because it can take advantage of the orthogonality of rotation matrices. On some platforms it can use SIMD instructions for further speedups.- Parameter
other
: RotationMatrix that post-multiplies
this
inverted.- Returns
R_BC
: where R_BC = this⁻¹ * other.
Note
It is possible (albeit improbable) to create an invalid rotation matrix by accumulating round-off error with a large number of multiplies.
- Parameter
- IsExactlyIdentity(self: pydrake.math.RotationMatrix) bool
Returns
True
ifthis
is exactly equal to the identity matrix.See also
IsNearlyIdentity().
- IsNearlyIdentity(self: pydrake.math.RotationMatrix, tolerance: float = 2.842170943040401e-14) bool
Returns true if
this
is within tolerance of the identity RigidTransform.- Parameter
tolerance
: non-negative number that is generally the default value, namely RotationMatrix::get_internal_tolerance_for_orthonormality().
See also
IsExactlyIdentity().
- Parameter
- IsValid(self: pydrake.math.RotationMatrix) bool
Tests if
this
rotation matrix R is a proper orthonormal rotation matrix to within the threshold of get_internal_tolerance_for_orthonormality().- Returns
True
ifthis
is a valid rotation matrix.
- static MakeFromOneVector(b_A: numpy.ndarray[numpy.float64[3, 1]], axis_index: int) pydrake.math.RotationMatrix
Creates a 3D right-handed orthonormal basis B from a given vector b_A, returned as a rotation matrix R_AB. It consists of orthogonal unit vectors u_A, v_A, w_A where u_A is the normalized b_A in the axis_index column of R_AB and v_A has one element which is zero. If an element of b_A is zero, then one element of w_A is 1 and the other two elements are 0.
- Parameter
b_A
: vector expressed in frame A that when normalized as u_A = b_A.normalized() represents Bx, By, or Bz (depending on axis_index).
- Parameter
axis_index
: The index ∈ {0, 1, 2} of the unit vector associated with u_A, 0 means u_A is Bx, 1 means u_A is By, and 2 means u_A is Bz.
- Precondition:
axis_index is 0 or 1 or 2.
- Raises
RuntimeError if b_A cannot be made into a unit vector because b_A –
contains a NaN or infinity or b_A < 1.0E-10. –
See also
MakeFromOneUnitVector() if b_A is known to already be unit length.
- Returns
R_AB
: the rotation matrix with properties as described above.
- Parameter
- static MakeXRotation(theta: float) pydrake.math.RotationMatrix
Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAx = Bx
.- Parameter
theta
: radian measure of rotation angle about Ax.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitX().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a right-handed rotation relative to A by an angletheta
aboutAx = Bx
.Click to expand C++ code...
⎡ 1 0 0 ⎤ R_AB = ⎢ 0 cos(theta) -sin(theta) ⎥ ⎣ 0 sin(theta) cos(theta) ⎦
- Parameter
- static MakeYRotation(theta: float) pydrake.math.RotationMatrix
Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAy = By
.- Parameter
theta
: radian measure of rotation angle about Ay.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitY().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a right-handed rotation relative to A by an angletheta
aboutAy = By
.Click to expand C++ code...
⎡ cos(theta) 0 sin(theta) ⎤ R_AB = ⎢ 0 1 0 ⎥ ⎣ -sin(theta) 0 cos(theta) ⎦
- Parameter
- static MakeZRotation(theta: float) pydrake.math.RotationMatrix
Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAz = Bz
.- Parameter
theta
: radian measure of rotation angle about Az.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitZ().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a right-handed rotation relative to A by an angletheta
aboutAz = Bz
.Click to expand C++ code...
⎡ cos(theta) -sin(theta) 0 ⎤ R_AB = ⎢ sin(theta) cos(theta) 0 ⎥ ⎣ 0 0 1 ⎦
- Parameter
- matrix(self: pydrake.math.RotationMatrix) numpy.ndarray[numpy.float64[3, 3]]
Returns the Matrix3 underlying a RotationMatrix.
See also
col(), row()
- multiply(*args, **kwargs)
Overloaded function.
multiply(self: pydrake.math.RotationMatrix, other: pydrake.math.RotationMatrix) -> pydrake.math.RotationMatrix
Calculates
this
rotation matrixR_AB
multiplied byother
rotation matrixR_BC
, returning the compositionR_AB * R_BC
.- Parameter
other
: RotationMatrix that post-multiplies
this
.
- Returns
rotation matrix that results from
this
multiplied byother
.
Note
It is possible (albeit improbable) to create an invalid rotation matrix by accumulating round-off error with a large number of multiplies.
multiply(self: pydrake.math.RotationMatrix, v_B: numpy.ndarray[numpy.float64[3, 1]]) -> numpy.ndarray[numpy.float64[3, 1]]
Calculates
this
rotation matrixR_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
.
multiply(self: pydrake.math.RotationMatrix, v_B: numpy.ndarray[numpy.float64[3, n]]) -> numpy.ndarray[numpy.float64[3, n]]
Multiplies
this
RotationMatrixR_AB
by the n vectorsv1
, … vn, where each vector has 3 elements and is expressed in frame B.- Parameter
v_B
: 3 x n
matrix whose n columns are regarded as arbitrary vectorsv1
, … vn expressed in frame B.- Returns
v_A
: 3 x n
matrix whose n columns are vectorsv1
, … vn expressed in frame A.
Click to expand C++ code...
const RollPitchYaw<double> rpy(0.1, 0.2, 0.3); const RotationMatrix<double> R_AB(rpy); Eigen::Matrix<double, 3, 2> v_B; v_B.col(0) = Vector3d(4, 5, 6); v_B.col(1) = Vector3d(9, 8, 7); const Eigen::Matrix<double, 3, 2> v_A = R_AB * v_B;
- static ProjectToRotationMatrix(M: numpy.ndarray[numpy.float64[3, 3]]) pydrake.math.RotationMatrix
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 toR * Rᵀ = I
, where I is the 3x3 identity matrix. For this problem, closeness can also be measured by forming the orthonormal matrix R whose elements minimize the double-summation∑ᵢ ∑ⱼ (R(i,j) - M(i,j))²
wherei = 1:3, j = 1:3
, subject toR * 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.
- Raises
RuntimeError if R fails IsValid(R) –
Note
William Kahan (UC Berkeley) and Hongkai Dai (Toyota Research Institute) proved that for this problem, the same R that minimizes the Frobenius norm also minimizes the 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‖
, whereu ≠ 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
- Parameter
- row(self: pydrake.math.RotationMatrix, index: int) numpy.ndarray[numpy.float64[1, 3]]
Returns
this
rotation matrix’s iᵗʰ row (i = 0, 1, 2). Forthis
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).
See also
col(), matrix()
- Raises
In Debug builds, asserts (0 <= index <= 2) –
Note
For efficiency and consistency with Eigen, this method returns the same quantity returned by Eigen’s row() operator. The returned quantity can be assigned in various ways, e.g., as
const auto& Az_B = row(2);
orRowVector3<T> Az_B = row(2);
- Parameter
- set(self: pydrake.math.RotationMatrix, R: numpy.ndarray[numpy.float64[3, 3]]) None
Sets
this
RotationMatrix from a Matrix3.- Parameter
R
: an allegedly valid rotation matrix.
- Raises
RuntimeError in debug builds if R fails IsValid(R) –
- Parameter
- ToAngleAxis(self: pydrake.math.RotationMatrix) pydrake.common.eigen_geometry.AngleAxis
Returns an AngleAxis
theta_lambda
containing an angletheta
and unit vector (axis direction)lambda
that representsthis
RotationMatrix.Note
The orientation and RotationMatrix associated with
theta * lambda
is identical to that of(-theta) * (-lambda)
. The AngleAxis returned by this method chooses to have0 <= theta <= pi
.- Returns
an AngleAxis with
0 <= theta <= pi
and a unit vectorlambda
.
- ToQuaternion(self: pydrake.math.RotationMatrix) pydrake.common.eigen_geometry.Quaternion
Returns a quaternion q that represents
this
RotationMatrix. Since the quaternionq
and-q
represent the same RotationMatrix, this method chooses to return a canonical quaternion, i.e., with q(0) >= 0.
- ToRollPitchYaw(self: pydrake.math.RotationMatrix) drake::math::RollPitchYaw<double>
Returns a RollPitchYaw that represents
this
RotationMatrix, with roll-pitch-yaw angles[r, p, y]
in the range-π <= r <= π
, -π/2 <= p <= π/2,-π <= y <= π
.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.
- transpose(self: pydrake.math.RotationMatrix) pydrake.math.RotationMatrix
- template pydrake.math.RotationMatrix_
Instantiations:
RotationMatrix_[float]
,RotationMatrix_[AutoDiffXd]
,RotationMatrix_[Expression]
- 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 isv_A = R_AB * v_B
, wherev_B
denotes an arbitrary vector v expressed in terms of Bx, By, Bz andv_A
denotes vector v expressed in terms of Ax, Ay, Az. See multibody_quantities for monogram notation for dynamics. See orientation_discussion “a discussion on rotation matrices”.Note
This class does not store the frames associated with a rotation matrix nor does it enforce strict proper usage of this class with vectors.
Note
When assertions are enabled, several methods in this class perform a validity check and throw RuntimeError if the rotation matrix is invalid. When assertions are disabled, many of these validity checks are skipped (which helps improve speed). These validity tests are only performed for scalar types for which drake::scalar_predicate<T>::is_bool is
True
. For instance, validity checks are not performed when T is symbolic::Expression.- Authors:
Paul Mitiguy (2018) Original author.
- Authors:
Drake team (see https://drake.mit.edu/credits).
- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.math.RotationMatrix_[AutoDiffXd]) -> None
Constructs a 3x3 identity RotationMatrix – which corresponds to aligning two frames (so that unit vectors Ax = Bx, Ay = By, Az = Bz).
__init__(self: pydrake.math.RotationMatrix_[AutoDiffXd], other: pydrake.math.RotationMatrix_[AutoDiffXd]) -> None
__init__(self: pydrake.math.RotationMatrix_[AutoDiffXd], R: numpy.ndarray[object[3, 3]]) -> None
Constructs a RotationMatrix from a Matrix3.
- Parameter
R
: an allegedly valid rotation matrix.
- Raises
RuntimeError in debug builds if R fails IsValid(R) –
__init__(self: pydrake.math.RotationMatrix_[AutoDiffXd], quaternion: pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd]) -> None
Constructs a RotationMatrix from an Eigen::Quaternion.
- Parameter
quaternion
: a 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.__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. –
__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]
.- Returns
R_AD
: , rotation matrix relating frame A to frame D.
Note
Denoting roll
r
, pitchp
, yawy
, this method returns a rotation matrixR_AD
equal to the matrix multiplication shown below.Click to expand C++ code...
⎡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
aboutDx = Cx
. Note: D and C are no longer aligned.2nd rotation R_BC: Frames D, C (collectively – as if welded together)
rotate relative to frame B, A by a pitch angle
p
aboutCy = By
. Note: C and B are no longer aligned.3rd rotation R_AB: Frames D, C, B (collectively – as if welded)
rotate relative to frame A by a roll angle
y
aboutBz = Az
. Note: B and A are no longer aligned.Note
This method constructs a RotationMatrix from a RollPitchYaw. Vice-versa, there are high-accuracy RollPitchYaw constructor/methods that form a RollPitchYaw from a rotation matrix.
- template cast
Instantiations:
cast[AutoDiffXd]
- cast[AutoDiffXd](self: pydrake.math.RotationMatrix_[AutoDiffXd]) pydrake.math.RotationMatrix_[AutoDiffXd]
Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T. For example,
Click to expand C++ code...
RotationMatrix<double> source = RotationMatrix<double>::Identity(); RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
- Template parameter
U
: Scalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>()
creates a newRotationMatrix<To>
from aRotationMatrix<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not vice-versa.- Template parameter
- col(self: pydrake.math.RotationMatrix_[AutoDiffXd], index: int) numpy.ndarray[object[3, 1]]
Returns
this
rotation matrix’s iᵗʰ column (i = 0, 1, 2). Forthis
rotation matrix R_AB (which relates 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).
See also
row(), matrix()
- Raises
In Debug builds, asserts (0 <= index <= 2) –
Note
For efficiency and consistency with Eigen, this method returns the same quantity returned by Eigen’s col() operator. The returned quantity can be assigned in various ways, e.g., as
const auto& Bz_A = col(2);
orVector3<T> Bz_A = col(2);
- Parameter
- static Identity() pydrake.math.RotationMatrix_[AutoDiffXd]
- inverse(self: pydrake.math.RotationMatrix_[AutoDiffXd]) pydrake.math.RotationMatrix_[AutoDiffXd]
- InvertAndCompose(self: pydrake.math.RotationMatrix_[AutoDiffXd], other: pydrake.math.RotationMatrix_[AutoDiffXd]) pydrake.math.RotationMatrix_[AutoDiffXd]
Calculates the product of
this
inverted and another RotationMatrix. If you considerthis
to be the rotation matrix R_AB, andother
to be R_AC, then this method returns R_BC = R_AB⁻¹ * R_AC. For T==double, this method can be much faster than inverting first and then performing the composition because it can take advantage of the orthogonality of rotation matrices. On some platforms it can use SIMD instructions for further speedups.- Parameter
other
: RotationMatrix that post-multiplies
this
inverted.- Returns
R_BC
: where R_BC = this⁻¹ * other.
Note
It is possible (albeit improbable) to create an invalid rotation matrix by accumulating round-off error with a large number of multiplies.
- Parameter
- IsExactlyIdentity(self: pydrake.math.RotationMatrix_[AutoDiffXd]) bool
Returns
True
ifthis
is exactly equal to the identity matrix.See also
IsNearlyIdentity().
- IsNearlyIdentity(self: pydrake.math.RotationMatrix_[AutoDiffXd], tolerance: float = 2.842170943040401e-14) bool
Returns true if
this
is within tolerance of the identity RigidTransform.- Parameter
tolerance
: non-negative number that is generally the default value, namely RotationMatrix::get_internal_tolerance_for_orthonormality().
See also
IsExactlyIdentity().
- Parameter
- IsValid(self: pydrake.math.RotationMatrix_[AutoDiffXd]) bool
Tests if
this
rotation matrix R is a proper orthonormal rotation matrix to within the threshold of get_internal_tolerance_for_orthonormality().- Returns
True
ifthis
is a valid rotation matrix.
- static MakeFromOneVector(b_A: numpy.ndarray[object[3, 1]], axis_index: int) pydrake.math.RotationMatrix_[AutoDiffXd]
Creates a 3D right-handed orthonormal basis B from a given vector b_A, returned as a rotation matrix R_AB. It consists of orthogonal unit vectors u_A, v_A, w_A where u_A is the normalized b_A in the axis_index column of R_AB and v_A has one element which is zero. If an element of b_A is zero, then one element of w_A is 1 and the other two elements are 0.
- Parameter
b_A
: vector expressed in frame A that when normalized as u_A = b_A.normalized() represents Bx, By, or Bz (depending on axis_index).
- Parameter
axis_index
: The index ∈ {0, 1, 2} of the unit vector associated with u_A, 0 means u_A is Bx, 1 means u_A is By, and 2 means u_A is Bz.
- Precondition:
axis_index is 0 or 1 or 2.
- Raises
RuntimeError if b_A cannot be made into a unit vector because b_A –
contains a NaN or infinity or b_A < 1.0E-10. –
See also
MakeFromOneUnitVector() if b_A is known to already be unit length.
- Returns
R_AB
: the rotation matrix with properties as described above.
- Parameter
- static MakeXRotation(theta: pydrake.autodiffutils.AutoDiffXd) pydrake.math.RotationMatrix_[AutoDiffXd]
Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAx = Bx
.- Parameter
theta
: radian measure of rotation angle about Ax.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitX().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a right-handed rotation relative to A by an angletheta
aboutAx = Bx
.Click to expand C++ code...
⎡ 1 0 0 ⎤ R_AB = ⎢ 0 cos(theta) -sin(theta) ⎥ ⎣ 0 sin(theta) cos(theta) ⎦
- Parameter
- static MakeYRotation(theta: pydrake.autodiffutils.AutoDiffXd) pydrake.math.RotationMatrix_[AutoDiffXd]
Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAy = By
.- Parameter
theta
: radian measure of rotation angle about Ay.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitY().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a right-handed rotation relative to A by an angletheta
aboutAy = By
.Click to expand C++ code...
⎡ cos(theta) 0 sin(theta) ⎤ R_AB = ⎢ 0 1 0 ⎥ ⎣ -sin(theta) 0 cos(theta) ⎦
- Parameter
- static MakeZRotation(theta: pydrake.autodiffutils.AutoDiffXd) pydrake.math.RotationMatrix_[AutoDiffXd]
Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAz = Bz
.- Parameter
theta
: radian measure of rotation angle about Az.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitZ().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a right-handed rotation relative to A by an angletheta
aboutAz = Bz
.Click to expand C++ code...
⎡ cos(theta) -sin(theta) 0 ⎤ R_AB = ⎢ sin(theta) cos(theta) 0 ⎥ ⎣ 0 0 1 ⎦
- Parameter
- matrix(self: pydrake.math.RotationMatrix_[AutoDiffXd]) numpy.ndarray[object[3, 3]]
Returns the Matrix3 underlying a RotationMatrix.
See also
col(), row()
- multiply(*args, **kwargs)
Overloaded function.
multiply(self: pydrake.math.RotationMatrix_[AutoDiffXd], other: pydrake.math.RotationMatrix_[AutoDiffXd]) -> pydrake.math.RotationMatrix_[AutoDiffXd]
Calculates
this
rotation matrixR_AB
multiplied byother
rotation matrixR_BC
, returning the compositionR_AB * R_BC
.- Parameter
other
: RotationMatrix that post-multiplies
this
.
- Returns
rotation matrix that results from
this
multiplied byother
.
Note
It is possible (albeit improbable) to create an invalid rotation matrix by accumulating round-off error with a large number of multiplies.
multiply(self: pydrake.math.RotationMatrix_[AutoDiffXd], v_B: numpy.ndarray[object[3, 1]]) -> numpy.ndarray[object[3, 1]]
Calculates
this
rotation matrixR_AB
multiplied by an arbitrary Vector3 expressed in the B frame.- Parameter
v_B
: 3x1 vector that post-multiplies
this
.
- Returns
3x1 vector
v_A = R_AB * v_B
.
multiply(self: pydrake.math.RotationMatrix_[AutoDiffXd], v_B: numpy.ndarray[object[3, n]]) -> numpy.ndarray[object[3, n]]
Multiplies
this
RotationMatrixR_AB
by the n vectorsv1
, … vn, where each vector has 3 elements and is expressed in frame B.- Parameter
v_B
: 3 x n
matrix whose n columns are regarded as arbitrary vectorsv1
, … vn expressed in frame B.- Returns
v_A
: 3 x n
matrix whose n columns are vectorsv1
, … vn expressed in frame A.
Click to expand C++ code...
const RollPitchYaw<double> rpy(0.1, 0.2, 0.3); const RotationMatrix<double> R_AB(rpy); Eigen::Matrix<double, 3, 2> v_B; v_B.col(0) = Vector3d(4, 5, 6); v_B.col(1) = Vector3d(9, 8, 7); const Eigen::Matrix<double, 3, 2> v_A = R_AB * v_B;
- static ProjectToRotationMatrix(M: numpy.ndarray[object[3, 3]]) pydrake.math.RotationMatrix_[AutoDiffXd]
Given an approximate rotation matrix M, finds the RotationMatrix R closest to M. Closeness is measured with a 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 toR * Rᵀ = I
, where I is the 3x3 identity matrix. For this problem, closeness can also be measured by forming the orthonormal matrix R whose elements minimize the double-summation∑ᵢ ∑ⱼ (R(i,j) - M(i,j))²
wherei = 1:3, j = 1:3
, subject toR * 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.
- Raises
RuntimeError if R fails IsValid(R) –
Note
William Kahan (UC Berkeley) and Hongkai Dai (Toyota Research Institute) proved that for this problem, the same R that minimizes the Frobenius norm also minimizes the 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‖
, whereu ≠ 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
- Parameter
- row(self: pydrake.math.RotationMatrix_[AutoDiffXd], index: int) numpy.ndarray[object[1, 3]]
Returns
this
rotation matrix’s iᵗʰ row (i = 0, 1, 2). Forthis
rotation matrix R_AB (which relates 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).
See also
col(), matrix()
- Raises
In Debug builds, asserts (0 <= index <= 2) –
Note
For efficiency and consistency with Eigen, this method returns the same quantity returned by Eigen’s row() operator. The returned quantity can be assigned in various ways, e.g., as
const auto& Az_B = row(2);
orRowVector3<T> Az_B = row(2);
- Parameter
- set(self: pydrake.math.RotationMatrix_[AutoDiffXd], R: numpy.ndarray[object[3, 3]]) None
Sets
this
RotationMatrix from a Matrix3.- Parameter
R
: an allegedly valid rotation matrix.
- Raises
RuntimeError in debug builds if R fails IsValid(R) –
- Parameter
- ToAngleAxis(self: pydrake.math.RotationMatrix_[AutoDiffXd]) pydrake.common.eigen_geometry.AngleAxis_[AutoDiffXd]
Returns an AngleAxis
theta_lambda
containing an angletheta
and unit vector (axis direction)lambda
that representsthis
RotationMatrix.Note
The orientation and RotationMatrix associated with
theta * lambda
is identical to that of(-theta) * (-lambda)
. The AngleAxis returned by this method chooses to have0 <= theta <= pi
.- Returns
an AngleAxis with
0 <= theta <= pi
and a unit vectorlambda
.
- ToQuaternion(self: pydrake.math.RotationMatrix_[AutoDiffXd]) pydrake.common.eigen_geometry.Quaternion_[AutoDiffXd]
Returns a quaternion q that represents
this
RotationMatrix. Since the quaternionq
and-q
represent the same RotationMatrix, this method chooses to return a canonical quaternion, i.e., with q(0) >= 0.
- ToRollPitchYaw(self: pydrake.math.RotationMatrix_[AutoDiffXd]) drake::math::RollPitchYaw<Eigen::AutoDiffScalar<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >
Returns a RollPitchYaw that represents
this
RotationMatrix, with roll-pitch-yaw angles[r, p, y]
in the range-π <= r <= π
, -π/2 <= p <= π/2,-π <= y <= π
.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.
- 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 isv_A = R_AB * v_B
, wherev_B
denotes an arbitrary vector v expressed in terms of Bx, By, Bz andv_A
denotes vector v expressed in terms of Ax, Ay, Az. See multibody_quantities for monogram notation for dynamics. See orientation_discussion “a discussion on rotation matrices”.Note
This class does not store the frames associated with a rotation matrix nor does it enforce strict proper usage of this class with vectors.
Note
When assertions are enabled, several methods in this class perform a validity check and throw RuntimeError if the rotation matrix is invalid. When assertions are disabled, many of these validity checks are skipped (which helps improve speed). These validity tests are only performed for scalar types for which drake::scalar_predicate<T>::is_bool is
True
. For instance, validity checks are not performed when T is symbolic::Expression.- Authors:
Paul Mitiguy (2018) Original author.
- Authors:
Drake team (see https://drake.mit.edu/credits).
- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.math.RotationMatrix_[Expression]) -> None
Constructs a 3x3 identity RotationMatrix – which corresponds to aligning two frames (so that unit vectors Ax = Bx, Ay = By, Az = Bz).
__init__(self: pydrake.math.RotationMatrix_[Expression], other: pydrake.math.RotationMatrix_[Expression]) -> None
__init__(self: pydrake.math.RotationMatrix_[Expression], R: numpy.ndarray[object[3, 3]]) -> None
Constructs a RotationMatrix from a Matrix3.
- Parameter
R
: an allegedly valid rotation matrix.
- Raises
RuntimeError in debug builds if R fails IsValid(R) –
__init__(self: pydrake.math.RotationMatrix_[Expression], quaternion: pydrake.common.eigen_geometry.Quaternion_[Expression]) -> None
Constructs a RotationMatrix from an Eigen::Quaternion.
- Parameter
quaternion
: a 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.__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. –
__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]
.- Returns
R_AD
: , rotation matrix relating frame A to frame D.
Note
Denoting roll
r
, pitchp
, yawy
, this method returns a rotation matrixR_AD
equal to the matrix multiplication shown below.Click to expand C++ code...
⎡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
aboutDx = Cx
. Note: D and C are no longer aligned.2nd rotation R_BC: Frames D, C (collectively – as if welded together)
rotate relative to frame B, A by a pitch angle
p
aboutCy = By
. Note: C and B are no longer aligned.3rd rotation R_AB: Frames D, C, B (collectively – as if welded)
rotate relative to frame A by a roll angle
y
aboutBz = Az
. Note: B and A are no longer aligned.Note
This method constructs a RotationMatrix from a RollPitchYaw. Vice-versa, there are high-accuracy RollPitchYaw constructor/methods that form a RollPitchYaw from a rotation matrix.
- template cast
Instantiations:
cast[Expression]
- cast[Expression](self: pydrake.math.RotationMatrix_[Expression]) pydrake.math.RotationMatrix_[Expression]
Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T. For example,
Click to expand C++ code...
RotationMatrix<double> source = RotationMatrix<double>::Identity(); RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
- Template parameter
U
: Scalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>()
creates a newRotationMatrix<To>
from aRotationMatrix<From>
but only if typeTo
is constructible from typeFrom
. This cast method works in accordance with Eigen’s cast method for Eigen’s Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not vice-versa.- Template parameter
- col(self: pydrake.math.RotationMatrix_[Expression], index: int) numpy.ndarray[object[3, 1]]
Returns
this
rotation matrix’s iᵗʰ column (i = 0, 1, 2). Forthis
rotation matrix R_AB (which relates 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).
See also
row(), matrix()
- Raises
In Debug builds, asserts (0 <= index <= 2) –
Note
For efficiency and consistency with Eigen, this method returns the same quantity returned by Eigen’s col() operator. The returned quantity can be assigned in various ways, e.g., as
const auto& Bz_A = col(2);
orVector3<T> Bz_A = col(2);
- Parameter
- static Identity() pydrake.math.RotationMatrix_[Expression]
- inverse(self: pydrake.math.RotationMatrix_[Expression]) pydrake.math.RotationMatrix_[Expression]
- InvertAndCompose(self: pydrake.math.RotationMatrix_[Expression], other: pydrake.math.RotationMatrix_[Expression]) pydrake.math.RotationMatrix_[Expression]
Calculates the product of
this
inverted and another RotationMatrix. If you considerthis
to be the rotation matrix R_AB, andother
to be R_AC, then this method returns R_BC = R_AB⁻¹ * R_AC. For T==double, this method can be much faster than inverting first and then performing the composition because it can take advantage of the orthogonality of rotation matrices. On some platforms it can use SIMD instructions for further speedups.- Parameter
other
: RotationMatrix that post-multiplies
this
inverted.- Returns
R_BC
: where R_BC = this⁻¹ * other.
Note
It is possible (albeit improbable) to create an invalid rotation matrix by accumulating round-off error with a large number of multiplies.
- Parameter
- IsExactlyIdentity(self: pydrake.math.RotationMatrix_[Expression]) pydrake.symbolic.Formula
Returns
True
ifthis
is exactly equal to the identity matrix.See also
IsNearlyIdentity().
- IsNearlyIdentity(self: pydrake.math.RotationMatrix_[Expression], tolerance: float = 2.842170943040401e-14) pydrake.symbolic.Formula
Returns true if
this
is within tolerance of the identity RigidTransform.- Parameter
tolerance
: non-negative number that is generally the default value, namely RotationMatrix::get_internal_tolerance_for_orthonormality().
See also
IsExactlyIdentity().
- Parameter
- IsValid(self: pydrake.math.RotationMatrix_[Expression]) pydrake.symbolic.Formula
Tests if
this
rotation matrix R is a proper orthonormal rotation matrix to within the threshold of get_internal_tolerance_for_orthonormality().- Returns
True
ifthis
is a valid rotation matrix.
- static MakeFromOneVector(b_A: numpy.ndarray[object[3, 1]], axis_index: int) pydrake.math.RotationMatrix_[Expression]
Creates a 3D right-handed orthonormal basis B from a given vector b_A, returned as a rotation matrix R_AB. It consists of orthogonal unit vectors u_A, v_A, w_A where u_A is the normalized b_A in the axis_index column of R_AB and v_A has one element which is zero. If an element of b_A is zero, then one element of w_A is 1 and the other two elements are 0.
- Parameter
b_A
: vector expressed in frame A that when normalized as u_A = b_A.normalized() represents Bx, By, or Bz (depending on axis_index).
- Parameter
axis_index
: The index ∈ {0, 1, 2} of the unit vector associated with u_A, 0 means u_A is Bx, 1 means u_A is By, and 2 means u_A is Bz.
- Precondition:
axis_index is 0 or 1 or 2.
- Raises
RuntimeError if b_A cannot be made into a unit vector because b_A –
contains a NaN or infinity or b_A < 1.0E-10. –
See also
MakeFromOneUnitVector() if b_A is known to already be unit length.
- Returns
R_AB
: the rotation matrix with properties as described above.
- Parameter
- static MakeXRotation(theta: pydrake.symbolic.Expression) pydrake.math.RotationMatrix_[Expression]
Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAx = Bx
.- Parameter
theta
: radian measure of rotation angle about Ax.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitX().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a right-handed rotation relative to A by an angletheta
aboutAx = Bx
.Click to expand C++ code...
⎡ 1 0 0 ⎤ R_AB = ⎢ 0 cos(theta) -sin(theta) ⎥ ⎣ 0 sin(theta) cos(theta) ⎦
- Parameter
- static MakeYRotation(theta: pydrake.symbolic.Expression) pydrake.math.RotationMatrix_[Expression]
Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAy = By
.- Parameter
theta
: radian measure of rotation angle about Ay.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitY().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a right-handed rotation relative to A by an angletheta
aboutAy = By
.Click to expand C++ code...
⎡ cos(theta) 0 sin(theta) ⎤ R_AB = ⎢ 0 1 0 ⎥ ⎣ -sin(theta) 0 cos(theta) ⎦
- Parameter
- static MakeZRotation(theta: pydrake.symbolic.Expression) pydrake.math.RotationMatrix_[Expression]
Makes the RotationMatrix
R_AB
associated with rotating a frame B relative to a frame A by an angletheta
about unit vectorAz = Bz
.- Parameter
theta
: radian measure of rotation angle about Az.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitZ().
Note
R_AB
relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially,Bx = Ax
, By = Ay,Bz = Az
, then B undergoes a right-handed rotation relative to A by an angletheta
aboutAz = Bz
.Click to expand C++ code...
⎡ cos(theta) -sin(theta) 0 ⎤ R_AB = ⎢ sin(theta) cos(theta) 0 ⎥ ⎣ 0 0 1 ⎦
- Parameter
- matrix(self: pydrake.math.RotationMatrix_[Expression]) numpy.ndarray[object[3, 3]]
Returns the Matrix3 underlying a RotationMatrix.
See also
col(), row()
- multiply(*args, **kwargs)
Overloaded function.
multiply(self: pydrake.math.RotationMatrix_[Expression], other: pydrake.math.RotationMatrix_[Expression]) -> pydrake.math.RotationMatrix_[Expression]
Calculates
this
rotation matrixR_AB
multiplied byother
rotation matrixR_BC
, returning the compositionR_AB * R_BC
.- Parameter
other
: RotationMatrix that post-multiplies
this
.
- Returns
rotation matrix that results from
this
multiplied byother
.
Note
It is possible (albeit improbable) to create an invalid rotation matrix by accumulating round-off error with a large number of multiplies.
multiply(self: pydrake.math.RotationMatrix_[Expression], v_B: numpy.ndarray[object[3, 1]]) -> numpy.ndarray[object[3, 1]]
Calculates
this
rotation matrixR_AB
multiplied by an arbitrary Vector3 expressed in the B frame.- Parameter
v_B
: 3x1 vector that post-multiplies
this
.
- Returns
3x1 vector
v_A = R_AB * v_B
.
multiply(self: pydrake.math.RotationMatrix_[Expression], v_B: numpy.ndarray[object[3, n]]) -> numpy.ndarray[object[3, n]]
Multiplies
this
RotationMatrixR_AB
by the n vectorsv1
, … vn, where each vector has 3 elements and is expressed in frame B.- Parameter
v_B
: 3 x n
matrix whose n columns are regarded as arbitrary vectorsv1
, … vn expressed in frame B.- Returns
v_A
: 3 x n
matrix whose n columns are vectorsv1
, … vn expressed in frame A.
Click to expand C++ code...
const RollPitchYaw<double> rpy(0.1, 0.2, 0.3); const RotationMatrix<double> R_AB(rpy); Eigen::Matrix<double, 3, 2> v_B; v_B.col(0) = Vector3d(4, 5, 6); v_B.col(1) = Vector3d(9, 8, 7); const Eigen::Matrix<double, 3, 2> v_A = R_AB * v_B;
- static ProjectToRotationMatrix(M: numpy.ndarray[object[3, 3]]) pydrake.math.RotationMatrix_[Expression]
Given an approximate rotation matrix M, finds the RotationMatrix R closest to M. Closeness is measured with a 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 toR * Rᵀ = I
, where I is the 3x3 identity matrix. For this problem, closeness can also be measured by forming the orthonormal matrix R whose elements minimize the double-summation∑ᵢ ∑ⱼ (R(i,j) - M(i,j))²
wherei = 1:3, j = 1:3
, subject toR * 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.
- Raises
RuntimeError if R fails IsValid(R) –
Note
William Kahan (UC Berkeley) and Hongkai Dai (Toyota Research Institute) proved that for this problem, the same R that minimizes the Frobenius norm also minimizes the 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‖
, whereu ≠ 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
- Parameter
- row(self: pydrake.math.RotationMatrix_[Expression], index: int) numpy.ndarray[object[1, 3]]
Returns
this
rotation matrix’s iᵗʰ row (i = 0, 1, 2). Forthis
rotation matrix R_AB (which relates 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).
See also
col(), matrix()
- Raises
In Debug builds, asserts (0 <= index <= 2) –
Note
For efficiency and consistency with Eigen, this method returns the same quantity returned by Eigen’s row() operator. The returned quantity can be assigned in various ways, e.g., as
const auto& Az_B = row(2);
orRowVector3<T> Az_B = row(2);
- Parameter
- set(self: pydrake.math.RotationMatrix_[Expression], R: numpy.ndarray[object[3, 3]]) None
Sets
this
RotationMatrix from a Matrix3.- Parameter
R
: an allegedly valid rotation matrix.
- Raises
RuntimeError in debug builds if R fails IsValid(R) –
- Parameter
- ToAngleAxis(self: pydrake.math.RotationMatrix_[Expression]) pydrake.common.eigen_geometry.AngleAxis_[Expression]
Returns an AngleAxis
theta_lambda
containing an angletheta
and unit vector (axis direction)lambda
that representsthis
RotationMatrix.Note
The orientation and RotationMatrix associated with
theta * lambda
is identical to that of(-theta) * (-lambda)
. The AngleAxis returned by this method chooses to have0 <= theta <= pi
.- Returns
an AngleAxis with
0 <= theta <= pi
and a unit vectorlambda
.
- ToQuaternion(self: pydrake.math.RotationMatrix_[Expression]) pydrake.common.eigen_geometry.Quaternion_[Expression]
Returns a quaternion q that represents
this
RotationMatrix. Since the quaternionq
and-q
represent the same RotationMatrix, this method chooses to return a canonical quaternion, i.e., with q(0) >= 0.
- ToRollPitchYaw(self: pydrake.math.RotationMatrix_[Expression]) drake::math::RollPitchYaw<drake::symbolic::Expression>
Returns a RollPitchYaw that represents
this
RotationMatrix, with roll-pitch-yaw angles[r, p, y]
in the range-π <= r <= π
, -π/2 <= p <= π/2,-π <= y <= π
.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.
- transpose(self: pydrake.math.RotationMatrix_[Expression]) pydrake.math.RotationMatrix_[Expression]
- pydrake.math.sin(*args, **kwargs)
Overloaded function.
sin(arg0: float) -> float
sin(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
sin(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.sinh(*args, **kwargs)
Overloaded function.
sinh(arg0: float) -> float
sinh(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
sinh(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.SoftOverMax(*args, **kwargs)
Overloaded function.
SoftOverMax(x: List[float], alpha: float = 1.0) -> float
Computes a smooth over approximation of max function, namely SoftOverMax(x) >= max(x). Mathematically we compute this as (log (∑ᵢ exp(αxᵢ))) / α.
- Parameter
x
: The vector for which we want to compute its soft max.
- Parameter
alpha
: α in the documentation above. Larger α makes the soft max more similar to max, with a sharper corner. Must be strictly positive and finite.
- Default: is 1.
$Raises:
RuntimeError if α <= 0.
- Raises
RuntimeError if α is non-finite. –
SoftOverMax(x: List[pydrake.autodiffutils.AutoDiffXd], alpha: float = 1.0) -> pydrake.autodiffutils.AutoDiffXd
Computes a smooth over approximation of max function, namely SoftOverMax(x) >= max(x). Mathematically we compute this as (log (∑ᵢ exp(αxᵢ))) / α.
- Parameter
x
: The vector for which we want to compute its soft max.
- Parameter
alpha
: α in the documentation above. Larger α makes the soft max more similar to max, with a sharper corner. Must be strictly positive and finite.
- Default: is 1.
$Raises:
RuntimeError if α <= 0.
- Raises
RuntimeError if α is non-finite. –
- pydrake.math.SoftOverMin(*args, **kwargs)
Overloaded function.
SoftOverMin(x: List[float], alpha: float = 1.0) -> float
Computes a smooth over approximation of min function, namely SoftOverMin(x) >= min(x). Mathematically we compute this as ∑ᵢ exp(-αxᵢ)*xᵢ / d, where d = ∑ⱼ exp(-αxⱼ)
- Parameter
x
: The vector for which we want to compute its soft min.
- Parameter
alpha
: α in the documentation above. Larger α makes the soft min more similar to min, with a sharper corner. Must be strictly positive and finite.
- Default: is 1.
$Raises:
RuntimeError if α <= 0.
- Raises
RuntimeError if α is non-finite. –
SoftOverMin(x: List[pydrake.autodiffutils.AutoDiffXd], alpha: float = 1.0) -> pydrake.autodiffutils.AutoDiffXd
Computes a smooth over approximation of min function, namely SoftOverMin(x) >= min(x). Mathematically we compute this as ∑ᵢ exp(-αxᵢ)*xᵢ / d, where d = ∑ⱼ exp(-αxⱼ)
- Parameter
x
: The vector for which we want to compute its soft min.
- Parameter
alpha
: α in the documentation above. Larger α makes the soft min more similar to min, with a sharper corner. Must be strictly positive and finite.
- Default: is 1.
$Raises:
RuntimeError if α <= 0.
- Raises
RuntimeError if α is non-finite. –
- pydrake.math.SoftUnderMax(*args, **kwargs)
Overloaded function.
SoftUnderMax(x: List[float], alpha: float = 1.0) -> float
Computes a smooth under approximation of max function, namely SoftUnderMax(x) <= max(x). Mathematically we compute this as ∑ᵢ exp(αxᵢ)*xᵢ / d, where d = ∑ⱼ exp(αxⱼ)
- Parameter
x
: The vector for which we want to compute its soft max.
- Parameter
alpha
: α in the documentation above. Larger α makes the soft max more similar to max, with a sharper corner. Must be strictly positive and finite.
- Default: is 1.
$Raises:
RuntimeError if α <= 0.
- Raises
RuntimeError if α is non-finite. –
SoftUnderMax(x: List[pydrake.autodiffutils.AutoDiffXd], alpha: float = 1.0) -> pydrake.autodiffutils.AutoDiffXd
Computes a smooth under approximation of max function, namely SoftUnderMax(x) <= max(x). Mathematically we compute this as ∑ᵢ exp(αxᵢ)*xᵢ / d, where d = ∑ⱼ exp(αxⱼ)
- Parameter
x
: The vector for which we want to compute its soft max.
- Parameter
alpha
: α in the documentation above. Larger α makes the soft max more similar to max, with a sharper corner. Must be strictly positive and finite.
- Default: is 1.
$Raises:
RuntimeError if α <= 0.
- Raises
RuntimeError if α is non-finite. –
- pydrake.math.SoftUnderMin(*args, **kwargs)
Overloaded function.
SoftUnderMin(x: List[float], alpha: float = 1.0) -> float
Computes a smooth under approximation of min function, namely SoftUnderMin(x) <= min(x). Mathematically we compute this as -(log (∑ᵢ exp(-αxᵢ))) / α
- Parameter
x
: The vector for which we want to compute its soft min.
- Parameter
alpha
: α in the documentation above. Larger α makes the soft min more similar to min, with a sharper corner. Must be strictly positive and finite.
- Default: is 1.
$Raises:
RuntimeError if α <= 0.
- Raises
RuntimeError if α is non-finite. –
SoftUnderMin(x: List[pydrake.autodiffutils.AutoDiffXd], alpha: float = 1.0) -> pydrake.autodiffutils.AutoDiffXd
Computes a smooth under approximation of min function, namely SoftUnderMin(x) <= min(x). Mathematically we compute this as -(log (∑ᵢ exp(-αxᵢ))) / α
- Parameter
x
: The vector for which we want to compute its soft min.
- Parameter
alpha
: α in the documentation above. Larger α makes the soft min more similar to min, with a sharper corner. Must be strictly positive and finite.
- Default: is 1.
$Raises:
RuntimeError if α <= 0.
- Raises
RuntimeError if α is non-finite. –
- pydrake.math.sqrt(*args, **kwargs)
Overloaded function.
sqrt(arg0: float) -> float
sqrt(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
sqrt(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.tan(*args, **kwargs)
Overloaded function.
tan(arg0: float) -> float
tan(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
tan(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.tanh(*args, **kwargs)
Overloaded function.
tanh(arg0: float) -> float
tanh(arg0: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
tanh(arg0: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
- pydrake.math.ToLowerTriangularColumnsFromMatrix(matrix: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) numpy.ndarray[numpy.float64[m, 1]]
Given a square matrix, extract the lower triangular part as a stacked column vector. This is a particularly useful operation when vectorizing symmetric matrices.
- pydrake.math.ToSymmetricMatrixFromLowerTriangularColumns(lower_triangular_columns: numpy.ndarray[numpy.float64[m, 1]]) numpy.ndarray[numpy.float64[m, n]]
Given a column vector containing the stacked columns of the lower triangular part of a square matrix, returning a symmetric matrix whose lower triangular part is the same as the original matrix.
- pydrake.math.UniformlyRandomAngleAxis(generator: pydrake.common.RandomGenerator) pydrake.common.eigen_geometry.AngleAxis
Generates a rotation (in the axis-angle representation) that rotates a point on the unit sphere to another point on the unit sphere with a uniform distribution over the sphere.
- pydrake.math.UniformlyRandomQuaternion(generator: pydrake.common.RandomGenerator) pydrake.common.eigen_geometry.Quaternion
Generates a rotation (in the quaternion representation) that rotates a point on the unit sphere to another point on the unit sphere with a uniform distribution over the sphere. This method is briefly explained in http://planning.cs.uiuc.edu/node198.html, a full explanation can be found in K. Shoemake. Uniform Random Rotations in D. Kirk, editor, Graphics Gems III, pages 124-132. Academic, New York, 1992.
- pydrake.math.UniformlyRandomRotationMatrix(generator: pydrake.common.RandomGenerator) drake::math::RotationMatrix<double>
Generates a rotation (in the rotation matrix representation) that rotates a point on the unit sphere to another point on the unit sphere with a uniform distribution over the sphere.
- pydrake.math.UniformlyRandomRPY(generator: pydrake.common.RandomGenerator) numpy.ndarray[numpy.float64[3, 1]]
Generates a rotation (in the roll-pitch-yaw representation) that rotates a point on the unit sphere to another point on the unit sphere with a uniform distribution over the sphere.
- pydrake.math.VectorToSkewSymmetric(*args, **kwargs)
Overloaded function.
VectorToSkewSymmetric(p: numpy.ndarray[numpy.float64[3, 1]]) -> numpy.ndarray[numpy.float64[3, 3]]
VectorToSkewSymmetric(p: numpy.ndarray[object[3, 1]]) -> numpy.ndarray[object[3, 3]]
VectorToSkewSymmetric(p: numpy.ndarray[object[3, 1]]) -> numpy.ndarray[object[3, 3]]
- pydrake.math.wrap_to(*args, **kwargs)
Overloaded function.
wrap_to(value: float, low: float, high: float) -> float
For variables that are meant to be periodic, (e.g. over a 2π interval), wraps
value
into the interval[low, high)
. Precisely,wrap_to
returns: value + k*(high-low) for the unique integer valuek
that lands the output in the desired interval.low
andhigh
must be finite, and low < high.wrap_to(value: pydrake.autodiffutils.AutoDiffXd, low: pydrake.autodiffutils.AutoDiffXd, high: pydrake.autodiffutils.AutoDiffXd) -> pydrake.autodiffutils.AutoDiffXd
For variables that are meant to be periodic, (e.g. over a 2π interval), wraps
value
into the interval[low, high)
. Precisely,wrap_to
returns: value + k*(high-low) for the unique integer valuek
that lands the output in the desired interval.low
andhigh
must be finite, and low < high.wrap_to(value: pydrake.symbolic.Expression, low: pydrake.symbolic.Expression, high: pydrake.symbolic.Expression) -> pydrake.symbolic.Expression
For variables that are meant to be periodic, (e.g. over a 2π interval), wraps
value
into the interval[low, high)
. Precisely,wrap_to
returns: value + k*(high-low) for the unique integer valuek
that lands the output in the desired interval.low
andhigh
must be finite, and low < high.