nabled_dynamics/
spatial.rs

1//! Spatial inertia and joint motion subspaces.
2
3use nabled_core::scalar::NabledReal;
4use nabled_model::joint::{JointAxis, JointType};
5use nabled_model::link::InertialSpec;
6use ndarray::{Array1, Array2, ArrayView1};
7
8/// Build spatial inertia from link inertial spec.
9pub fn from_inertial_spec<T: NabledReal>(spec: &InertialSpec<T>) -> SpatialInertia<T> {
10    SpatialInertia {
11        mass:    spec.mass,
12        com:     ndarray::arr1(&spec.com),
13        inertia: spec.inertia.clone(),
14    }
15}
16
17/// 6×6 spatial inertia matrix via the parallel-axis theorem.
18pub fn spatial_inertia_6x6<T: NabledReal>(spec: &InertialSpec<T>) -> Array2<T> {
19    let m = spec.mass;
20    let c = ndarray::arr1(&spec.com);
21    let i3 = &spec.inertia;
22    let cx = nabled_linalg::geometry::so3::hat(&c.view());
23    let mcx = cx.mapv(|v| v * m);
24    let top_left = i3 + &mcx.t().dot(&cx);
25    let top_right = mcx.t();
26    let bottom_left = mcx.clone();
27    let mut spatial = Array2::<T>::zeros((6, 6));
28    for i in 0..3 {
29        for j in 0..3 {
30            spatial[[i, j]] = top_left[[i, j]];
31            spatial[[i, j + 3]] = top_right[[i, j]];
32            spatial[[i + 3, j]] = bottom_left[[i, j]];
33            spatial[[i + 3, j + 3]] = if i == j { m } else { T::zero() };
34        }
35    }
36    spatial
37}
38
39/// Apply spatial inertia to a motion vector: `f = I v`.
40pub fn spatial_inertia_apply<T: NabledReal>(
41    inertia: &Array2<T>,
42    v: &ArrayView1<'_, T>,
43) -> Array1<T> {
44    inertia.dot(v)
45}
46
47/// Transform spatial inertia: `I' = Ad^T I Ad`.
48pub fn spatial_inertia_transform<T: NabledReal>(
49    inertia: &Array2<T>,
50    adjoint: &Array2<T>,
51) -> Array2<T> {
52    adjoint.t().dot(inertia).dot(adjoint)
53}
54
55/// Joint motion subspace `S` (6-vector) for revolute/prismatic joints.
56pub fn joint_motion_subspace<T: NabledReal>(joint_type: JointType, axis: JointAxis) -> Array1<T> {
57    let unit = axis.unit_vector::<T>();
58    let mut s = Array1::<T>::zeros(6);
59    match joint_type {
60        JointType::Revolute => {
61            s[0] = unit[0];
62            s[1] = unit[1];
63            s[2] = unit[2];
64        }
65        JointType::Prismatic => {
66            s[3] = unit[0];
67            s[4] = unit[1];
68            s[5] = unit[2];
69        }
70        JointType::Fixed => {}
71    }
72    s
73}
74
75/// Spatial gravity acceleration vector `[0; g]`.
76pub fn spatial_gravity<T: NabledReal>(gravity: &[T; 3]) -> Array1<T> {
77    let mut accel = Array1::<T>::zeros(6);
78    accel[3] = gravity[0];
79    accel[4] = gravity[1];
80    accel[5] = gravity[2];
81    accel
82}
83
84/// Apply motion cross product via geometry twist helper.
85pub fn motion_cross_product<T: NabledReal>(
86    a: &ArrayView1<'_, T>,
87    b: &ArrayView1<'_, T>,
88) -> Array1<T> {
89    nabled_linalg::geometry::twist::motion_cross(a, b)
90}
91
92/// Apply force cross product via geometry twist helper.
93pub fn force_cross_product<T: NabledReal>(
94    a: &ArrayView1<'_, T>,
95    b: &ArrayView1<'_, T>,
96) -> Array1<T> {
97    nabled_linalg::geometry::twist::force_cross(a, b)
98}
99
100#[derive(Debug, Clone, PartialEq)]
101pub struct SpatialInertia<T> {
102    /// Link mass.
103    pub mass:    T,
104    /// Center of mass.
105    pub com:     Array1<T>,
106    /// 3×3 inertia tensor.
107    pub inertia: Array2<T>,
108}
109
110#[cfg(test)]
111mod tests {
112    use approx::assert_relative_eq;
113
114    use super::*;
115
116    #[test]
117    fn point_mass_inertia_diagonal() {
118        let spec = InertialSpec {
119            mass:    2.0_f64,
120            com:     [0.5, 0.0, 0.0],
121            inertia: Array2::<f64>::zeros((3, 3)),
122        };
123        let i6 = spatial_inertia_6x6(&spec);
124        assert_relative_eq!(i6[[5, 5]], 2.0, epsilon = 1e-12);
125        assert_relative_eq!(i6[[4, 4]], 2.0, epsilon = 1e-12);
126    }
127
128    #[test]
129    fn spatial_inertia_apply_matches_hand_calc() {
130        let spec = InertialSpec {
131            mass:    1.0_f64,
132            com:     [0.0, 0.0, 0.0],
133            inertia: Array2::<f64>::eye(3),
134        };
135        let i6 = spatial_inertia_6x6(&spec);
136        let v = ndarray::arr1(&[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
137        let f = spatial_inertia_apply(&i6, &v.view());
138        assert_relative_eq!(f[0], 1.0, epsilon = 1e-12);
139    }
140
141    #[test]
142    fn revolute_z_subspace() {
143        let s = joint_motion_subspace::<f64>(JointType::Revolute, JointAxis::Z);
144        assert_relative_eq!(s[2], 1.0, epsilon = 1e-12);
145    }
146}
nabled_dynamics/spatial.rs

nabled_dynamics/
spatial.rs
