Inertia

Overview

In the body center of mass frame, the \(6 \times 6\) Inertia matrix, \(M\), can be expressed as:

(1)\[\begin{split}M_{11} = \begin{bmatrix} mI & 0\\ 0 & \bar{I} \end{bmatrix}\end{split}\]

Where \(m\) is the body mass, \(I\) is a \(3 \times 3\) identity matrix and \(\bar{I}\) is the \(3 \times 3\) rotational inertia matrix. The body center of mass frame is denoted with a subscript \(c\). When solving the equations of motion in a fixed inertial reference frame, it is necessary to transform these local inertia matrices to the origin of the fixed inertial reference frame, which is denoted with a subscript \(f\). To understand how the inertia matrix is transformed to other coordinate systems, consider the equation for momentum, \(\underline{\mathbf{p}}^*\). Note that the asterisk indicates the \(\underline{\mathbf{p}}^*\) is a coscrew quantity.

(2)\[\underline{\mathbf{p}}^*_f = M_{cc} \underline{\mathbf{v}}_c\]

As noted above, coscrew quantities are transformed by left multiplication with the coadjoint operator, \([Ad]^*\).

(3)\[[Ad]^*_{cf}\underline{\mathbf{p}}_f = M_{cc}[Ad]_{fc}^{-1}[Ad]_{fc}\underline{\mathbf{v}}_c\]
(4)\[\underline{\mathbf{p}}_f = [Ad]^{*-1}_{10}M_{11}[Ad]^{-1}_{fc}{\underline{\mathbf{v}}}_f= [Ad]^*_{fc}M_{cc}[Ad]_{cf}\underline{\mathbf{v}}_f\]

yielding:

(5)\[M_{ff} = [Ad]^*_{fc}M_{cc}[Ad]_{cf}\]

Again the \(6 \times 6\) Adjoint and Coadjoint matrices provides a convenient and compact notation to describe transformations, but full dense matrix algebra need not be applied to carry out transformation of the inertia matrix, noting that:

(6)\[\begin{split}M_{ff} = [Ad]^*_{fc}M_{cc}[Ad]_{cf} = \begin{bmatrix} R_{fc}m_cIR_{cf} & R_{fc}m_cI\tilde{r}_{cf}R_{cf}\\ \tilde{r}_{cf}R_{cf}m_1IR_{cf}& \tilde{r}_{cf}R_{cf}m_cI\tilde{r}_{cf}R_{cf} + R_{fc}\bar{I}_{cc}R_{cf} \end{bmatrix}\end{split}\]

which can be further simplified as:

(7)\[\begin{split}M_{ff} = \begin{bmatrix} m_cI & m_cR_{fc}\tilde{r}_{cf}R_{cf}\\ -m_cR_{fc}\tilde{r}_{cf}R_{cf} & \bar{I}_{cc} -m_c\tilde{r}_{cf}^2 \end{bmatrix}\end{split}\]

and:

(8)\[\begin{split}M_{ff} = \begin{bmatrix} m_cI & m_c\tilde{r}_{cf}\\ -m_c\tilde{r}_{cf} & \bar{I}_{cc} -m_c\tilde{r}_{cf}^2 \end{bmatrix}\end{split}\]

Based on Eq. (8), it is only necessary to store ten terms to reconstruct the inertia matrix: \(m_c, m_c\underline{r}\), and the terms on and below the diagonal of \(\bar{I}\) (i.e. \(I_{xx}\), \(I_{yy}\), \(I_{zz}\), \(I_{xy}\), \(I_{yz}\) and \(I_{zx}\)). Storing only these 10 terms requires approximately 70% less storage than the full \(6\times6\) inertia matrix. In order to take advantage of SIMD for various algorithms which access the quantities in the inertia matrix, additional memory is used to store the quantity \(-m_c\underline{r}\), which still results in a 64% reduction in storage compared to the full \(6 \times 6\) representation.

If the reference frame for the body is not taken as the center of mass, then in addition to the inertia matrix stored in the center of mass coordinate system, a vector from the body’s reference frame to the center of mass frame, \(\underline{r}^{bc}_b\), is stored. Here, the subscript \(b\) denotes the body reference frame and the \(c\) denotes the center of mass frame.

(9)\[M_{ff} = [Ad]^{*}_{fb}[Ad]^{*}_{bc} M_{cc} [Ad]_{cb}[Ad]_{bf}\]
(10)\[M_{ff} = R_{fb} \bar{I}_{bb}^{cc} R_{bf}\]
(11)\[\underline{r}_{f}^{fc} = \underline{r}_{f}^{fb} + R_{fb} \underline{r}_{b}^{bc}\]
(12)\[\underline{t}_{temp} = m_c \underline{r}_{f}^{fc}\]
(13)\[\begin{split}M_{ff} = \begin{bmatrix} mI & -\tilde{t}_{temp}\\ \tilde{t}_{temp} & \bar{I}^{ff}_{cc} - \tilde{t}_{temp} \tilde{t}^{f}_{fc}\end{bmatrix}\end{split}\]

Then use the following identity to simplify the matrix product \(\tilde{t}_{temp} \tilde{t}^{f}_{fc}\):

(14)\[\tilde{a} \tilde{b} = b a^T - (a^T b) I\]

Therefore, \(\tilde{a} \tilde{b}\) is found as:

(15)\[\begin{split}\begin{array} \text{result} \leftarrow \begin{Bmatrix}b_0 \\ b_1 \\ b_2\end{Bmatrix} \begin{Bmatrix}a_0&a_1&a_2\end{Bmatrix}\\ result -= trace(result) * I \end{array}\end{split}\]
(16)\[\bar{I}^{ff}_{ff} = \bar{I}^{ff}_{cc} - \tilde{t}_{temp} \tilde{r}^{f}_{fc}\]
(17)\[\bar{I}^{ff}_{ff} = \bar{I}^{ff}_{cc} - result\]

Double precision

union ksl_inertia_t
#include <inertia.h>

double precision mass and inertia properties

Public Members

double at[10]
struct ksl_inertia_t::[anonymous] [anonymous]

anonymous union allows accessing by index

double m

mass

ksl_vec3_t mt

mass times a vector, t, from reference frame to body centroid

double Ixx

Ixx centroidal inertia term, expressed in reference frame coordinates

double Iyy

Ixy centroidal inertia term, expressed in reference frame coordinates

double Izz

Izz centroidal inertia term, expressed in reference frame coordinates

double Ixy

Ixy centroidal inertia term, expressed in reference frame coordinates

double Iyz

Iyz centroidal inertia term, expressed in reference frame coordinates

double Izx

Izx centroidal inertia term, expressed in reference frame coordinates

struct ksl_inertia_t::[anonymous] [anonymous]

anonymous union allows accessing by field names

double mass

mass

ksl_vec3_t mr

mass times a vector, r, from reference frame to body centroid

double _Ixx

Ixx centroidal inertia term, expressed in reference frame coordinates

double _Iyy

Ixy centroidal inertia term, expressed in reference frame coordinates

double _Izz

Izz centroidal inertia term, expressed in reference frame coordinates

double Iyx

Ixy centroidal inertia term, expressed in reference frame coordinates

double Izy

Iyz centroidal inertia term, expressed in reference frame coordinates

double Ixz

Izx centroidal inertia term, expressed in reference frame coordinates

struct ksl_inertia_t::[anonymous] [anonymous]

anonymous union allows accessing by alternative field names

ksl_inertia_t ksl_inertia(const double m, const ksl_vec3_t t, const double Ixx, const double Iyy, const double Izz, const double Ixy, const double Iyz, const double Izx)

double precision spatial inertia matrix constructor

Parameters
  • m: [in] mass
  • t: [in] vector from reference frame to body centroid
  • Ixx: [in] centroidal inertia term, expressed in reference frame coordinates
  • Iyy: [in] centroidal inertia term, expressed in reference frame coordinates
  • Izz: [in] centroidal inertia term, expressed in reference frame coordinates
  • Ixy: [in] centroidal inertia term, expressed in reference frame coordinates
  • Iyz: [in] centroidal inertia term, expressed in reference frame coordinates
  • Izx: [in] centroidal inertia term, expressed in reference frame coordinates

ksl_inertia_t *ksl_inertia_alloc(int n)

Allocate n ksl_inertia_t datastructures on the heap.

void ksl_inertia_rotated(const ksl_inertia_t *inertia_i, const ksl_mat3x3_t *r, ksl_inertia_t *inertia_o)

Rotate a double precision centroidal inertia matrix.

\( [Ad]^*_{(\Phi(R^{fc}))} M_{cc} [Ad]_{(\Phi((R^{fc})^{-1}))} \rightarrow M_{ff}\)

Parameters
  • inertia_i: [in] input inertia matrix
  • r: [in] orthonormal rotation matrix
  • inertia_o: [out] output inertia matrix

void ksl_inertia_rotate(ksl_inertia_t *inertia_i, const ksl_mat3x3_t *r)

Rotate a double precision centroidal inertia matrix in place.

\( [Ad]^*_{(\Phi(R^{fc}))} M_{cc} [Ad]_{(\Phi((R^{fc})^{-1}))} \rightarrow M_{ff}\)

Parameters
  • inertia_i: [in/out] input inertia matrix
  • r: [in] orthonormal rotation matrix

void ksl_inertia_translated(const ksl_inertia_t *inertia_i, const ksl_vec3_t *t, ksl_inertia_t *inertia_o)

translate a double precision centroidal inertia matrix

\( [Ad]^*_{(\Phi(\underline{t}_{fc}^f))} M_{cc} [Ad]_{(\Phi((\underline{t}_{fc}^f)^{-1}))} \rightarrow M_{ff}\)

Parameters
  • inertia_i: [in] input inertia matrix
  • t: [in] translation vector
  • inertia_o: [out] output inertia matrix

void ksl_inertia_translate(ksl_inertia_t *inertia_i, const ksl_vec3_t *t)

translate a double precision centroidal inertia matrix in place

\( [Ad]^*_{(\Phi(\underline{t}_{fc}^f))} M_{cc} [Ad]_{(\Phi((\underline{t}_{fc}^f)^{-1}))} \rightarrow M_{ff}\)

Parameters
  • inertia_i: [in/out] input inertia matrix
  • t: [in] translation vector

void ksl_inertia_transformed(const ksl_inertia_t *inertia_i, const ksl_SE3_t *d, ksl_inertia_t *inertia_o)

spatial transform a double precision centroidal inertia matrix

\( [Ad]_{(\Phi_{fc})}^* M_{cc} [Ad]_{(\Phi_{fc}^{-1})} \rightarrow M_{ff}\)

Parameters
  • inertia_i: [in] input inertia matrix
  • d: [in] SE3 transformation matrix
  • inertia_o: [out] output inertia matrix

void ksl_inertia_transform(ksl_inertia_t *inertia_i, const ksl_SE3_t *d)

Perform a spatial transformation of a double precision centroidal inertia matrix in place.

\( [Ad]_{(\Phi_{fc})}^* M_{cc} [Ad]_{(\Phi_{fc}^{-1})} \rightarrow M_{ff}\)

Parameters
  • inertia_i: [in/out] inertia matrix to transform
  • d: [in] SE3 transformation

void ksl_inertia_merge(ksl_inertia_t *inertia_i, ksl_vec3_t *t_ic_i, ksl_inertia_t *inertia_j, const ksl_vec3_t *t_jc_j, const ksl_SE3_t *D_ij)

merge double precision child inertia (j) with parent inertia (i)

Parameters
  • parent: inertia [in/out] parent inertia to be merged
  • t_ic_i: [in/out] vector from parent body reference frame i to body center of mass frame c, expressed in body frame i
  • child: inertia [in]
  • t_jc_j: [in] vector from child body reference frame j to body center of mass frame c, expressed in body frame j
  • D_ij: [in] transformation from parent reference frame i to child reference frame j

int ksl_inertia_factor(ksl_inertia_t *)

factor double precision inertia matrix

inertia matrix is overwritten with its factors

4 cases: rank 2+, full rank, positive definite, or rank 2, positive semidefinite rank 1, positive semidefinite rank 0, do nothing

Return
rank of inertia matrix

Single precision

union ksl_inertiaf_t
#include <inertia.h>

single precision mass and inertia properties

Public Members

float at[10]
struct ksl_inertiaf_t::[anonymous] [anonymous]

anonymous union allows accessing by index

float m

mass

ksl_vec3f_t mt

mass times a vector, t, from reference frame to body centroid

float Ixx

Ixx centroidal inertia term, expressed in reference frame coordinates

float Iyy

Ixy centroidal inertia term, expressed in reference frame coordinates

float Izz

Izz centroidal inertia term, expressed in reference frame coordinates

float Ixy

Ixy centroidal inertia term, expressed in reference frame coordinates

float Iyz

Iyz centroidal inertia term, expressed in reference frame coordinates

float Izx

Izx centroidal inertia term, expressed in reference frame coordinates

struct ksl_inertiaf_t::[anonymous] [anonymous]

anonymous union allows accessing inertial quantities by field name

float mass

mass

ksl_vec3f_t mr

mass times a vector, r, from reference frame to body centroid

float _Ixx

Ixx centroidal inertia term, expressed in reference frame coordinates

float _Iyy

Ixy centroidal inertia term, expressed in reference frame coordinates

float _Izz

Izz centroidal inertia term, expressed in reference frame coordinates

float Iyx

Ixy centroidal inertia term, expressed in reference frame coordinates

float Izy

Iyz centroidal inertia term, expressed in reference frame coordinates

float Ixz

Izx centroidal inertia term, expressed in reference frame coordinates

struct ksl_inertiaf_t::[anonymous] [anonymous]

anonymous union allows accessing inertial quantities by alternative field name

ksl_inertiaf_t ksl_inertiaf(const float m, const ksl_vec3f_t t, const float Ixx, const float Iyy, const float Izz, const float Ixy, const float Iyz, const float Izx)

single precision spatial inertia matrix constructor

Parameters
  • m: [in] mass
  • t: [in] vector from reference frame to body centroid
  • Ixx: [in] centroidal inertia term, expressed in reference frame coordinates
  • Iyy: [in] centroidal inertia term, expressed in reference frame coordinates
  • Izz: [in] centroidal inertia term, expressed in reference frame coordinates
  • Ixy: [in] centroidal inertia term, expressed in reference frame coordinates
  • Iyz: [in] centroidal inertia term, expressed in reference frame coordinates
  • Izx: [in] centroidal inertia term, expressed in reference frame coordinates

ksl_inertiaf_t *ksl_inertiaf_alloc(int n)

Allocate n ksl_inertiaf_t datastructures on the heap.

void ksl_inertiaf_rotated(const ksl_inertiaf_t *inertia_i, const ksl_mat3x3f_t *r, ksl_inertiaf_t *inertia_o)

Rotate a single precision centroidal inertia matrix.

\( [Ad]^*_{(\Phi(R^{fc}))} M_{cc} [Ad]_{(\Phi((R^{fc})^{-1}))} \rightarrow M_{ff}\)

Parameters
  • inertia_i: [in] input inertia matrix
  • r: [in] orthonormal rotation matrix
  • inertia_o: [out] output inertia matrix

void ksl_inertiaf_rotate(ksl_inertiaf_t *inertia_i, const ksl_mat3x3f_t *r)

Rotate a single precision centroidal inertia matrix in place.

\( [Ad]^*_{(\Phi(R^{fc}))} M_{cc} [Ad]_{(\Phi((R^{fc})^{-1}))} \rightarrow M_{ff}\)

Parameters
  • inertia_i: [in/out] input inertia matrix
  • r: [in] orthonormal rotation matrix

void ksl_inertiaf_translated(const ksl_inertiaf_t *inertia_i, const ksl_vec3f_t *t, ksl_inertiaf_t *inertia_o)

translate a single precision centroidal inertia matrix

\( [Ad]^*_{(\Phi(\underline{t}_{fc}^f))} M_{cc} [Ad]_{(\Phi((\underline{t}_{fc}^f)^{-1}))} \rightarrow M_{ff}\)

Parameters
  • inertia_i: [in] input inertia matrix
  • t: [in] translation vector
  • inertia_o: [out] output inertia matrix

void ksl_inertiaf_translate(ksl_inertiaf_t *inertia_i, const ksl_vec3f_t *t)

translate a single precision centroidal inertia matrix in place

\( [Ad]^*_{(\Phi(\underline{t}_{fc}^f))} M_{cc} [Ad]_{(\Phi((\underline{t}_{fc}^f)^{-1}))} \rightarrow M_{ff}\)

Parameters
  • inertia_i: [in/out] input inertia matrix
  • t: [in] translation vector

void ksl_inertiaf_transformed(const ksl_inertiaf_t *inertia_i, const ksl_SE3f_t *d, ksl_inertiaf_t *inertia_o)

spatial transform a single precision centroidal inertia matrix

\( [Ad]_{(\Phi_{fc})}^* M_{cc} [Ad]_{(\Phi_{fc}^{-1})} \rightarrow M_{ff}\)

Parameters
  • inertia_i: [in] input inertia matrix
  • d: [in] SE3 transformation matrix
  • inertia_o: [out] output inertia matrix

void ksl_inertiaf_transform(ksl_inertiaf_t *inertia_i, const ksl_SE3f_t *d)

Perform a spatial transformation of a single precision centroidal inertia matrix in place.

\( [Ad]_{(\Phi_{fc})}^* M_{cc} [Ad]_{(\Phi_{fc}^{-1})} \rightarrow M_{ff}\)

Parameters
  • inertia_i: [in/out] inertia matrix to transform
  • d: [in] SE3 transformation

void ksl_inertiaf_merge(ksl_inertiaf_t *inertia_i, ksl_vec3f_t *t_ic_i, ksl_inertiaf_t *inertia_j, const ksl_vec3f_t *t_jc_j, const ksl_SE3f_t *D_ij)

merge single precision child inertia (j) with parent inertia (i)

Parameters
  • parent: inertia [in/out] parent inertia to be merged
  • t_ic_i: [in/out] vector from parent body reference frame i to body center of mass frame c, expressed in body frame i
  • child: inertia [in]
  • t_jc_j: [in] vector from child body reference frame j to body center of mass frame c, expressed in body frame j
  • D_ij: [in] transformation from parent reference frame i to child reference frame j

int ksl_inertiaf_factor(ksl_inertiaf_t *)

factor single precision inertia matrix

inertia matrix is overwritten with its factors

4 cases: rank 2+, full rank, positive definite, or rank 2, positive semidefinite rank 1, positive semidefinite rank 0, do nothing

Return
rank of inertia matrix