phoxal 0.41.3

Phoxal - production-oriented autonomous robot framework: the runtime engine and model (the api contract tree lives in phoxal-api, the typed bus in phoxal-bus).
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63

//! 3D transform helper used by model config and TF payloads.

use nalgebra::{Quaternion, UnitQuaternion};
use serde::{Deserialize, Serialize};

/// 3D transform (translation + rotation).
#[derive(Clone, Copy, Debug, Serialize, Deserialize)]
pub struct Transform {
    /// Translation in meters [x, y, z].
    pub translation_xyz: [f64; 3],
    /// Rotation in radians [roll, pitch, yaw].
    pub rotation_rpy: [f64; 3],
}

impl Transform {
    pub const IDENTITY: Self = Self {
        translation_xyz: [0.0, 0.0, 0.0],
        rotation_rpy: [0.0, 0.0, 0.0],
    };

    #[must_use]
    pub const fn new(translation: [f64; 3], rotation: [f64; 3]) -> Self {
        Self {
            translation_xyz: translation,
            rotation_rpy: rotation,
        }
    }

    /// Convert RPY rotation to quaternion [x, y, z, w].
    #[must_use]
    pub fn rotation_quaternion(&self) -> [f64; 4] {
        let [roll, pitch, yaw] = self.rotation_rpy;
        let quaternion = UnitQuaternion::from_euler_angles(roll, pitch, yaw);
        [quaternion.i, quaternion.j, quaternion.k, quaternion.w]
    }

    /// Create transform from translation and quaternion [x, y, z, w].
    #[must_use]
    pub fn from_translation_quaternion(translation: [f64; 3], quaternion: [f64; 4]) -> Self {
        let [x, y, z, w] = quaternion;
        let unit = UnitQuaternion::new_normalize(Quaternion::new(w, x, y, z));
        let (roll, pitch, yaw) = unit.euler_angles();
        Self {
            translation_xyz: translation,
            rotation_rpy: [roll, pitch, yaw],
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn quaternion_identity() {
        let transform = Transform::IDENTITY;
        let quaternion = transform.rotation_quaternion();
        assert!((quaternion[0] - 0.0).abs() < 1e-10);
        assert!((quaternion[1] - 0.0).abs() < 1e-10);
        assert!((quaternion[2] - 0.0).abs() < 1e-10);
        assert!((quaternion[3] - 1.0).abs() < 1e-10);
    }
}