dynamics_spatial/
inertia.rs

1//! Defines spatial **inertia** and related operations.
2
3use nalgebra::Matrix6;
4
5use crate::{motion::SpatialMotion, vector6d::Vector6D};
6use std::ops::Mul;
7
8#[derive(Debug, Clone, PartialEq, Default)]
9/// Spatial inertia matrix, represented as a 6x6 matrix.
10///
11/// The spatial inertia matrix is expressed as:
12/// $$\begin{bmatrix} `mI_3` & -mc_\times \\\\ mc_\times & `I_c` \end{bmatrix}$$
13/// where $m$ is the mass, $`I_3`$ is the 3x3 identity matrix,
14/// $c_\times=\[c\]_\times$ is the skew-symmetric matrix of the center
15/// of mass vector, and $`I_c`$ is the rotational inertia matrix about the center of mass.
16pub struct SpatialInertia(Matrix6<f64>);
17
18impl SpatialInertia {
19    /// Creates a new `SpatialInertia` object with the given elements.
20    ///
21    /// # Arguments
22    ///
23    /// * `ixx`, `iyy`, `izz` - Diagonal elements of the rotational inertia matrix.
24    /// * `ixy`, `ixz`, `iyz` - Off-diagonal elements of the rotational inertia matrix.
25    ///
26    /// # Returns
27    /// A new `SpatialInertia` object.
28    #[must_use]
29    pub fn new(ixx: f64, ixy: f64, ixz: f64, iyy: f64, iyz: f64, izz: f64) -> Self {
30        let mut mat = Matrix6::<f64>::zeros();
31        mat[(0, 0)] = ixx;
32        mat[(0, 1)] = ixy;
33        mat[(0, 2)] = ixz;
34        mat[(1, 0)] = ixy;
35        mat[(1, 1)] = iyy;
36        mat[(1, 2)] = iyz;
37        mat[(2, 0)] = ixz;
38        mat[(2, 1)] = iyz;
39        mat[(2, 2)] = izz;
40        Self(mat)
41    }
42
43    /// Creates a new `SpatialInertia` object with all elements set to zero.
44    #[must_use]
45    pub fn zeros() -> Self {
46        Self(Matrix6::<f64>::zeros())
47    }
48
49    /// Creates a new `SpatialInertia` object from a diagonal vector.
50    ///
51    /// # Arguments
52    ///
53    /// * `diag` - A 6D vector representing the diagonal elements of the spatial inertia matrix.
54    ///
55    /// # Returns
56    /// A new `SpatialInertia` object.
57    #[must_use]
58    pub fn from_diagonal(diag: &Vector6D) -> Self {
59        Self(diag.as_diagonal())
60    }
61}
62
63impl Mul<&SpatialMotion> for &SpatialInertia {
64    type Output = SpatialMotion;
65
66    fn mul(self, rhs: &SpatialMotion) -> Self::Output {
67        SpatialMotion(self.0 * rhs.0)
68    }
69}
dynamics_spatial/inertia.rs

dynamics_spatial/
inertia.rs
