dair_pll.inertia

Utilities for transforming representations of rigid body inertia.

The inertial parameterization of a body \(B\) is given by 10 degrees of freedom:

\(m\) (1 DoF), the mass.
\(^{Bo}p^{Bcm}\) (3 DoF), the position of the center of mass \(Bcm\) relative to the body’s origin:
p_BoBcm_B == [p_x, p_y, p_z]
\(I^{B/Bcm}\) (6 DoF), the symmetric 3x3 inertia dyadic about the body’s center of mass \(Bcm\):
I_BBcm_B == [[I_xx, I_xy, I_xz],[I_xy, I_yy, I_yz],[I_xz, I_yz, I_zz]]

Here we list and several formats that can be converted between each other freely, under the assumption that their values are non-degenerate and valid ( i.e. \(m > 0\)).

pi_cm is an intuitive formatting of these 10 Dof as a standard vector in \(\mathbb{R}^{10}\) as:
[m, m * p_x, m * p_y, m * p_z, I_xx, I_yy, I_zz, I_xy, I_xz, I_yz]
pi_o is nearly the same as pi_cm except that the moments of inertia are about the body’s origin \(Bo\) instead of the center of mass.
drake_spatial_inertia is a scaling that can be used to construct a new Drake SpatialInertia, where the inertial tensor is normalized by mass (see UnitInertia):
[m, p_x, p_y, p_z, ...
    I_xx / m, I_yy / m, I_zz / m, I_xy / m, I_xz / m, I_yz / m]
drake is a packaging of drake_spatial_inertia into a Drake SpatialInertia object, with member callbacks to access the terms:
SpatialInertia.get_mass() == m
SpatialInertia.get_com() == p_BoBcm_B
SpatialInertia.CalcRotationalInertia() == I_BBo_B
SpatialInertia.Shift(p_BoBcm_B).CalcRotationalInertia() == I_BBcm_B
theta is a format designed for underlying smooth, unconstrained, and non-degenerate parameterization of rigid body inertia. For a body, any value in \(\mathbb{R}^{10}\) for theta can be mapped to a valid and non-degenerate set of inertial terms as follows:
theta == [alpha, d_1, d_2, d_3, s_12, s_23, s_13, t_1, t_2, t_3]
s == [s_12, s_23, s_13]
t == [t_1, t_2, t_3]
pi_o == [
    t \cdot t + 1,
    t_1 * exp(d_1),
    t_1 * s_12 + t_2 * exp(d_2),
    t_1 * s_13 + t_2 * s_23 + t_3 * exp(d_3),
    s \cdot s + exp(2 * d_2) + exp(2 * d_3),
    s_13 ** 2 + s_23 ** 2 + exp(2 * d_1) + exp(2 * d_3),
    s_12 ** 2 + exp(2 * d_1) + exp(2 * d_2),
    -s_12 * exp(d_1),
    -s_13 * exp(d_1),
    -s_12 * s_13 - s_23 * exp(d_2)
]
An original derivation and characterization of theta can be found in Rucker and Wensing [1]. Note that Drake orders the inertial off-diagonal terms as [Ixy Ixz Iyz], whereas the original paper [1] uses [Ixy Iyz Ixz]; thus the index ordering here is slightly different.
urdf is the string format in which inertial parameters are stored, represented as the tuple:
"m", "p_x p_y p_z", ["I_xx", "I_yy", "I_zz", "I_xy", "I_xz", "I_yz"]
scalars is the string dictionary format for printing on tensorboard:
{"m": m, "p_x": p_x, ... "I_yz": I_yz}

Various transforms between these types are implemented in this module through the InertialParameterConverter class.

dair_pll.inertia.number_to_float(number)[source]

Converts a number to float via intermediate string representation.

Return type:: float

dair_pll.inertia.parallel_axis_theorem(I_BBa_B, m_B, p_BaBb_B, Ba_is_Bcm=True)[source]

Converts an inertia matrix represented from one reference point to that represented from another reference point. One of these reference points must be the center of mass.

The parallel axis theorem states [2]:

\[I_R = I_C - m_{tot} [d]^2\]

…for \(I_C\) as the inertia matrix about the center of mass, \(I_R\) as the moment of inertia about a point \(R\) defined as \(R = C + d\), and \(m_{tot}\) as the total mass of the body. The brackets in \([d]\) indicate the skew-symmetric matrix formed from the vector \(d\).

[2] https://en.wikipedia.org/wiki/Moment_of_inertia#Parallel_axis_theorem

Parameters:

I_BBa_B (Tensor) – (*, 3, 3) inertia matrices.
m_B (Tensor) – (*) masses.
p_BaBb_B (Tensor) – (*, 3) displacement from current frame to new frame.
Ba_is_Bcm (bool) – True if the provided I_BBa_B is from the perspective of the CoM, False if from the perspective of the origin.

Return type:

Tensor

Returns:

(*, 3, 3) inertia matrices with changed reference point.

dair_pll.inertia.inertia_matrix_from_vector(I_BBa_B_vec)[source]

Converts vectorized inertia vector of the following order into an inertia matrix:

\[\begin{split}[I_{xx}, I_{yy}, I_{zz}, I_{xy}, I_{xz}, I_{yz}] \Rightarrow \begin{bmatrix} I_{xx} & I_{xy} & I_{xz} \\ I_{xy} & I_{yy} & I_{yz} \\ I_{xz} & I_{yz} & I_{zz} \end{bmatrix}\end{split}\]

Parameters:: I_BBa_B_vec (Tensor) – (*, 6) vectorized inertia parameters.
Return type:: Tensor
Returns:: (*, 3, 3) inertia matrix.

dair_pll.inertia.inertia_vector_from_matrix(I_BBa_B_mat)[source]

Converts inertia matrix into vectorized inertia vector of the following order:

\[\begin{split}\begin{bmatrix} I_{xx} & I_{xy} & I_{xz} \\ I_{xy} & I_{yy} & I_{yz} \\ I_{xz} & I_{yz} & I_{zz} \end{bmatrix} \Rightarrow [I_{xx}, I_{yy}, I_{zz}, I_{xy}, I_{xz}, I_{yz}]\end{split}\]

Parameters:: I_BBa_B_mat (Tensor) – (*, 3, 3) inertia matrix.
Return type:: Tensor
Returns:: (*, 6) vectorized inertia parameters.

class dair_pll.inertia.InertialParameterConverter[source]

Bases: object

Utility class for transforming between inertial parameterizations.

static theta_to_pi_o(theta)[source]

Converts batch of theta parameters to pi_o parameters.

Parameters:: theta (Tensor) – (*, 10) theta-type parameterization.
Return type:: Tensor
Returns:: (*, 10) pi_o-type parameterization.

static pi_o_to_theta(pi_o)[source]

Converts batch of pi_o parameters to theta parameters.

Implements hand-derived local inverse of standard mapping from Rucker and Wensing.

This function inverts theta_to_pi_o() for valid pi_o.

Parameters:: pi_o (Tensor) – (*, 10) pi_o-type parameterization.
Return type:: Tensor
Returns:: (*, 10) theta-type parameterization.

static pi_o_to_pi_cm(pi_o)[source]

Converts batch of pi_o parameters to pi_cm parameters using the parallel axis theorem.

Parameters:: pi_o (Tensor) – (*, 10) pi_o-type parameterization.
Returns:: (*, 10) pi_cm-type parameterization.
Return type:: pi_cm

static pi_cm_to_pi_o(pi_cm)[source]

Converts batch of pi_cm parameters to pi_o parameters using the parallel axis theorem.

Parameters:: pi_cm (Tensor) – (*, 10) pi_cm-type parameterization.
Returns:: (*, 10) pi_o-type parameterization.
Return type:: pi_o

static theta_to_pi_cm(theta)[source]

Passthrough chain of theta_to_pi_o() and pi_o_to_pi_cm().

Return type:: Tensor

static pi_cm_to_theta(pi_cm)[source]

Passthrough chain of pi_cm_to_pi_o() and pi_o_to_theta().

Return type:: Tensor

static pi_cm_to_drake_spatial_inertia(pi_cm)[source]

Converts batch of pi-cm parameters to drake_spatial_inertia parameters.

Return type:: Tensor

static pi_cm_to_urdf(pi_cm)[source]

Converts a single (10,) pi_cm vector into the urdf string format.

Return type:: Tuple[str, str, List[str]]

static drake_to_pi_cm(spatial_inertia)[source]

Extracts a pi-cm parameterization from a Drake SpatialInertia object.

Return type:: Tensor

static drake_to_pi_o(spatial_inertia)[source]

Extracts a pi-o parameterization from a Drake SpatialInertia object.

Return type:: Tensor

static drake_inertial_components_to_pi(mass, p_BoBcm_B, I_BBa_B)[source]

Combines system mass with Drake representations of CoM location and inertia parameters (w.r.t. either the body origin or CoM) into a Tensor of pi parameters. Note: If Ba is Bo, the returned pi is pi_o; similarly, if Ba is Bcm, the returned pi is pi_cm.

Return type:: Tensor

static drake_to_theta(spatial_inertia)[source]

Passthrough chain of drake_to_pi_o() and pi_o_to_theta().

Return type:: Tensor

static pi_cm_to_scalars(pi_cm)[source]

Converts pi_cm parameterization to scalars dictionary.

Return type:: Dict[str, float]