dair_pll.tensor_utils
Tensor utility functions.
Contains various utility functions for common tensor operations required throughout the package. All such future functions should be placed here, with the following exceptions:
Utilities for operating directly on \(SO(3)\) should be placed in
dair_pll.quaternion
- dair_pll.tensor_utils.tile_dim(tiling_tensor, copies, dim=0)[source]
Tiles tensor along specified dimension.
- Parameters:
- Return type:
- Returns:
(n_0, ..., n * n_dim, ... n_{k-1})tiled tensor.- Raises:
ValueError – when
copiesis not a strictly-positive integer
- dair_pll.tensor_utils.tile_last_dim(tiling_tensor, copies)[source]
Tile right dimension (
-1) viatile_dim()- Return type:
- dair_pll.tensor_utils.tile_penultimate_dim(tiling_tensor, copies)[source]
Tile second-to-last dimension (
-2)tile_dim()- Return type:
- dair_pll.tensor_utils.pbmm(t_1, t_2)[source]
Multiplies matrices with optional batching.
Wrapper function that performs a final-axes (
-2,-1) matrix-matrix, vector-matrix, matrix-vector, or vector-vector product depending on the shape oft_1andt_2. The following logic is used:do a matrix-matrix multiplication if both factors have dimension at least two, and broadcast to the larger (inferred) batch.
do a vector-matrix / matrix-vector multiplication if one factor is a vector and the other has dimension
>= 2do a vector-vector multiplication if both factors are vectors.
- dair_pll.tensor_utils.deal(dealing_tensor, dim=0, keep_dim=False)[source]
Converts dim of tensor to list.
Example
Let
tbe a 3-dimensional tensor of shape(3,5,3)such that:t[:, i, :] == torch.eye(3).
Then
deal(t, dim=1)returns a list of 5(3,3)identity tensors, anddeal(t, dim=1, keep_dim=True)returns a list of(3,1,3)tensors.- Parameters:
- Return type:
- Returns:
List of dealt sub-tensors of shape
(..., n_{dim-1}, {n_dim+1}, ...)or(..., n_{dim-1}, 1, {n_dim+1}, ...).
- dair_pll.tensor_utils.skew_symmetric(vectors)[source]
Converts vectors in \(\mathbb{R}^3\) into skew-symmetric form.
Converts vector(s) \(v\) in
vectorsinto skew-symmetric matrix:\[\begin{split}S(v) = -S(v)^T = \begin{bmatrix} 0 & -v_3 & v_2 \\ v_3 & 0 & -v_1 \\ -v_2 & v_1 & 0 \end{bmatrix}\end{split}\]
- dair_pll.tensor_utils.symmetric_offdiagonal(vectors)[source]
Converts vectors in \(\mathbb{R}^3\) into symmetric off-diagonal form. This is the same as skew symmetric except for the skew negative signs.
Converts vector(s) \(v\) in
vectorsinto symmetric matrix:\[\begin{split}S(v) = S(v)^T = \begin{bmatrix} 0 & v_3 & v_2 \\ v_3 & 0 & v_1 \\ v_2 & v_1 & 0 \end{bmatrix}\end{split}\]
- dair_pll.tensor_utils.batch_diagonal(vectors)[source]
Converts vectors to diagonal matrices.
Take in an arbitrary batch of n-vectors and returns the same sized batch of (n, n) matrices such that:
output[b_1, ..., b_k, :, :] == torch.diag(vectors[b_1, ..., b_k, :]).
Code structure comes from thw following address:
- dair_pll.tensor_utils.one_vector_block_diagonal(num_blocks, vector_length)[source]
Computes a block diagonal matrix with column vectors of ones as blocks.
Associated with the mathematical symbol \(E\).
Example
one_vector_block_diagonal(3, 2) == tensor([ [1., 0., 0.], [1., 0., 0.], [0., 1., 0.], [0., 1., 0.], [0., 0., 1.], [0., 0., 1.]]).
- dair_pll.tensor_utils.spatial_to_point_jacobian(p_BoP_E)[source]
Body-fixed translational velocity to spatial velocity Jacobian.
Takes a batch of points \([^{Bo}p^P]_E\) fixed to body \(B\) and expressed in some coordinates \(E\), and constructs the Jacobian of their linear velocity in some other frame \(A\) w.r.t. the \(E\)-coordinate spatial velocity of \(B\) relative to \(A\).
In detail, let the \(i`th element of the batch represent point ``Pi`\) as \([^{Bo}p^{Pi}]_E\), and let Ao be fixed in A. The Jacobian calculated is
\[J = \frac{\partial [^Av^{Pi}]_E }{\partial [^AV^B]_E}.\]We have that \([^AV^B]_E = [^A\omega^B; ^{A}v^{Bo}]_E\), and from kinematics that
\[^Av^{Pi}= ^{A}v^{Bo} + ^A\omega^B \times ^{Bo}p^{Pi}.\]Thus, the Jacobian is of the form
\[J = [-S([^{Bo}p^{Pi}]_E), I_3],\]where \(S\) is calculated via
skew_symmetric().
- dair_pll.tensor_utils.rotation_matrix_from_one_vector(directions, axis)[source]
Converts a batch of directions for specified axis, to a batch of rotation matrices.
Specifically, if the
ith provided direction isd_i, then theith returned rotation matrixR_iobeys:R_i[:, axis] == d_i.
Reimplements the algorithm from Drake’s
pydrake.math.RotationMatrix_[float].MakeFromOneVector(). For more details, seerotation_matrix.cc:L13at the following address:
- dair_pll.tensor_utils.broadcast_lorentz(vectors)[source]
Utility function that broadcasts scalars into Lorentz product cone format.
This function maps a given vector \(v = [v_1, \dots, v_n]\) in given batch
vectorsto\[\begin{bmatrix} v & v_1 & v_1 & \cdots & v_n & v_n \end{bmatrix}.\]
- dair_pll.tensor_utils.project_lorentz(vectors)[source]
Utility function that projects vectors in Lorentz cone product.
This function takes in a batch of vectors
\[\begin{split}\begin{align} v &= \begin{bmatrix} v_{n1} & \cdots v_{nk} & v_{t1} & \cdots v_{tk} \end{bmatrix},\\ v_{ni} &\in \mathbb{R},\\ v_{ti} &\in \mathbb{R}^2,\\ \end{align}\end{split}\]and projects each \(v_i = [v_{ni} v_{ti}]\) into the Lorentz cone \(L = \{ v_{ni} \geq ||v_{ti}||_2\}\) via the following piecewise formula:
if \(v_i \in L\), it remains the same.
if \(v_i \in L^{\circ} = \{-v_{ni} \geq ||v_{ti}||_2\}\) (the polar cone), replace it with \(0\).
if \(v_i \not\in L \cup L^\circ\), replace it with
\[v = \begin{bmatrix} n & \frac{n}{||v_{ti}||_2}v_{ti} \end{bmatrix},\]where \(n = \frac{1}{2}(v_{ni} + ||v_{ti}||_2)\).
- dair_pll.tensor_utils.sappy_reorder_mat(n_cones)[source]
Generates a 0-1 matrix that reorders force variable indices between
dair_pllordering andsappyordering.dair_pllorders force variables as\[\lambda = \begin{bmatrix} \lambda_{n1}; & \cdots \lambda_{nk}; & \lambda_{t1}; & \cdots \lambda_{tk}; \end{bmatrix}\,,\]whereas
sappyaccepts decision variables in format\[\lambda_s = \begin{bmatrix} \lambda_{t1}; & \lambda_{n1}; & \cdots & \lambda_{tk}; & \lambda_{nk} \end{bmatrix}\,.\]This function returns matrix \(M\) to map betweens the two as
\[\lambda = M\lambda_s\]