nabled_model/
origin.rs

1//! Joint origin transforms from URDF-style parameters.
2
3use nabled_core::scalar::NabledReal;
4use nabled_linalg::geometry::{Rotation3, Transform3, se3, so3};
5use ndarray::{Array1, Array2, arr1};
6
7use crate::ModelError;
8
9/// Build a rigid transform from URDF `<origin xyz="..." rpy="..."/>`.
10///
11/// Rotation uses fixed-axis roll (X), pitch (Y), yaw (Z): `R = Rz * Ry * Rx`.
12pub fn transform_from_xyz_rpy<T: NabledReal>(
13    xyz: [T; 3],
14    rpy: [T; 3],
15) -> Result<Transform3<T>, ModelError> {
16    let rotation = rotation_from_urdf_rpy(rpy)?;
17    let translation = arr1(&[xyz[0], xyz[1], xyz[2]]);
18    Ok(se3::from_rotation_translation(&rotation, &translation))
19}
20
21/// Map legacy DH scalar fields stored on [`BodySpec`](crate::robot::BodySpec) to a static origin.
22pub fn joint_origin_from_dh_scalars<T: NabledReal>(
23    a: T,
24    alpha: T,
25    d: T,
26    theta: T,
27) -> Result<Transform3<T>, ModelError> {
28    transform_from_xyz_rpy([a, T::zero(), d], [alpha, theta, T::zero()])
29}
30
31/// Identity transform (parent and child frames coincident).
32#[must_use]
33pub fn identity_transform<T: NabledReal>() -> Transform3<T> {
34    se3::from_rotation_translation(
35        &Rotation3 { matrix: Array2::<T>::eye(3) },
36        &Array1::<T>::zeros(3),
37    )
38}
39
40fn rotation_from_urdf_rpy<T: NabledReal>(rpy: [T; 3]) -> Result<Rotation3<T>, ModelError> {
41    let rx = so3::exp(&arr1(&[rpy[0], T::zero(), T::zero()]).view())
42        .map_err(|_| ModelError::InvalidInput("invalid roll angle".to_string()))?;
43    let ry = so3::exp(&arr1(&[T::zero(), rpy[1], T::zero()]).view())
44        .map_err(|_| ModelError::InvalidInput("invalid pitch angle".to_string()))?;
45    let rz = so3::exp(&arr1(&[T::zero(), T::zero(), rpy[2]]).view())
46        .map_err(|_| ModelError::InvalidInput("invalid yaw angle".to_string()))?;
47    let ry_rx = so3::compose(&ry, &rx)
48        .map_err(|_| ModelError::InvalidInput("invalid origin rotation".to_string()))?;
49    so3::compose(&rz, &ry_rx)
50        .map_err(|_| ModelError::InvalidInput("invalid origin rotation".to_string()))
51}
52
53#[cfg(test)]
54mod tests {
55    use approx::assert_relative_eq;
56
57    use super::*;
58
59    #[test]
60    fn pure_translation_origin() {
61        let tf = transform_from_xyz_rpy([1.0_f64, 0.0, 0.0], [0.0, 0.0, 0.0]).unwrap();
62        assert_relative_eq!(tf.translation[0], 1.0, epsilon = 1e-12);
63    }
64
65    #[test]
66    fn identity_transform_is_identity() {
67        let tf = identity_transform::<f64>();
68        assert_relative_eq!(tf.translation[0], 0.0, epsilon = 1e-12);
69        assert_relative_eq!(tf.rotation.matrix[[0, 0]], 1.0, epsilon = 1e-12);
70    }
71
72    #[test]
73    fn joint_origin_from_dh_scalars_matches_translation() {
74        let tf = joint_origin_from_dh_scalars(1.0_f64, 0.0, 0.5, 0.0).unwrap();
75        assert_relative_eq!(tf.translation[0], 1.0, epsilon = 1e-12);
76        assert_relative_eq!(tf.translation[2], 0.5, epsilon = 1e-12);
77    }
78
79    #[test]
80    fn yaw_rotation_rotates_x_into_y() {
81        let tf =
82            transform_from_xyz_rpy([0.0_f64, 0.0, 0.0], [0.0, 0.0, std::f64::consts::FRAC_PI_2])
83                .unwrap();
84        let point = se3::transform_point(&tf, &arr1(&[1.0_f64, 0.0, 0.0]).view());
85        assert_relative_eq!(point[0], 0.0, epsilon = 1e-10);
86        assert_relative_eq!(point[1], 1.0, epsilon = 1e-10);
87    }
88}
nabled_model/origin.rs

nabled_model/
origin.rs
