engeom 0.5.0

Library for working with geometry with a focus on engineering metrology applications.
Documentation
use crate::na::{Translation3, UnitQuaternion};
use crate::{Iso3, Vector3};
use serde::{Deserialize, Serialize};

/// A struct representing a 6D pose in XYZ and WPR (Yaw, Pitch, Roll) format, commonly used by
/// FANUC robots. Angles are represented in degrees.
#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
pub struct XyzWpr {
    pub x: f64,
    pub y: f64,
    pub z: f64,
    pub w: f64,
    pub p: f64,
    pub r: f64,
}

impl XyzWpr {
    pub fn new(x: f64, y: f64, z: f64, w: f64, p: f64, r: f64) -> Self {
        XyzWpr { x, y, z, w, p, r }
    }

    pub fn approx_eq(&self, other: &XyzWpr, epsilon: f64) -> bool {
        let m0 = Iso3::from(self).to_matrix();
        let m1 = Iso3::from(other).to_matrix();
        let diff = m0 - m1;
        diff.amax() < epsilon
    }

    pub fn to_array(&self) -> [f64; 6] {
        [self.x, self.y, self.z, self.w, self.p, self.r]
    }
}

impl std::fmt::Display for XyzWpr {
    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
        write!(
            f,
            "XyzWpr {{ x: {}, y: {}, z: {}, w: {}, p: {}, r: {} }}",
            self.x, self.y, self.z, self.w, self.p, self.r
        )
    }
}

impl From<[f64; 6]> for XyzWpr {
    fn from(value: [f64; 6]) -> Self {
        XyzWpr::new(value[0], value[1], value[2], value[3], value[4], value[5])
    }
}

impl From<&Iso3> for XyzWpr {
    fn from(isometry: &Iso3) -> Self {
        let translation = isometry.translation.vector;
        let rotation = isometry.rotation;
        let (w, p, r) = rotation.euler_angles();
        XyzWpr::new(
            translation.x,
            translation.y,
            translation.z,
            w.to_degrees(),
            p.to_degrees(),
            r.to_degrees(),
        )
    }
}

impl From<&XyzWpr> for Iso3 {
    fn from(value: &XyzWpr) -> Self {
        let translation = Vector3::new(value.x, value.y, value.z);

        let rotation = UnitQuaternion::from_euler_angles(
            value.w.to_radians(),
            value.p.to_radians(),
            value.r.to_radians(),
        );

        Iso3::from_parts(Translation3::from(translation), rotation)
    }
}

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

    use crate::na::{Matrix4, try_convert};
    use approx::assert_relative_eq;
    use test_case::test_case;

    #[test_case(151.8, 677.1, 277.0, 119.2, -0.2, -179.6)]
    #[test_case(451.0, 265.4, -242.9, 179.8, -0.0, 90.1)]
    #[test_case(450.9, 62.5, -242.5, -179.9, 0.0, 90.4)]
    #[test_case(508.7, -253.6, -197.2, -179.3, -0.4, 82.2)]
    #[test_case(-413.8635232282, 263.1863811434, 291.9820746748, -96.8338333769, -35.3450905433, 179.9682178248)]
    #[test_case(-149.1668440821, 59.3543506644, 543.2838251943, -120.8798246395, 16.7001441168, 99.0130388728)]
    #[test_case(31.0642288750, -26.7743169615, 37.9006405971, -39.6352782596, -2.9214064154, -59.8943255222)]
    #[test_case(33.5138781088, 96.1164814542, -300.9032245106, -90.3561134473, 24.0876647389, -5.3733485621)]
    #[test_case(77.8658952620, 100.2923343848, -127.6541936118, -171.7171410357, -14.6451288996, 99.8404139441)]
    #[test_case(-70.6389144769, 22.3217390074, 18.1798498130, 23.2569815201, 57.7013782986, -140.4311743532)]
    #[test_case(604.2872268462, -20.9333741686, 30.4479677368, 114.8155367373, 31.6840511268, -151.5220050073)]
    #[test_case(77.4016768659, -28.4720503689, -19.7908480100, 85.1619317507, 53.8517426985, 34.2917400153)]
    #[test_case(471.9320306822, -102.6726730677, 588.5120154719, -49.5782300627, -31.1318310910, -84.4135682965)]
    #[test_case(-577.6445766741, -216.2517731052, 240.9460890915, -63.0050045001, 75.3203125033, -172.1668871906)]
    #[test_case(411.6803610302, 331.0434986878, -361.9951518329, -63.7156683382, 45.0090193165, 75.3251405039)]
    #[test_case(5.4890201453, -26.5744088848, -99.6628429743, -179.9170519309, 12.2738553488, -139.1333357015)]
    #[test_case(-1.7434467919, -14.7633944063, -79.9344894886, 39.4698348110, 28.9693530939, -103.4665369616)]
    #[test_case(-49.7118498703, 20.3765636776, 8.4422995188, -68.5366961539, -27.7591729450, 35.2043221840)]
    #[test_case(59.5794643289, 6.7548483372, -88.8341095469, -146.2807856754, 1.0526398756, -16.7799423973)]
    fn xyzwpr_round_trip(x: f64, y: f64, z: f64, w: f64, p: f64, r: f64) {
        let xyzwpr = XyzWpr::new(x, y, z, w, p, r);
        let iso = Iso3::from(&xyzwpr);
        let xyzwpr_converted = XyzWpr::from(&iso);
        assert_relative_eq!(xyzwpr.x, xyzwpr_converted.x, epsilon = 1e-8);
        assert_relative_eq!(xyzwpr.y, xyzwpr_converted.y, epsilon = 1e-8);
        assert_relative_eq!(xyzwpr.z, xyzwpr_converted.z, epsilon = 1e-8);
        assert_relative_eq!(xyzwpr.w, xyzwpr_converted.w, epsilon = 1e-8);
        assert_relative_eq!(xyzwpr.p, xyzwpr_converted.p, epsilon = 1e-8);
        assert_relative_eq!(xyzwpr.r, xyzwpr_converted.r, epsilon = 1e-8);
    }

    // These first four test cases are taken from an actual FANUC robot, the rest were from test
    // cases generated by a trusted robot simulator.
    #[test_case((-0.9999695385, -0.0003588828, -0.0077970028, 151.8000000000, -0.0069812178, 0.4878690427, 0.8728889161, 677.1000000000, 0.0034906514, 0.8729167591, -0.4878566869, 277.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (151.8000000000, 677.1000000000, 277.0000000000, 119.2000000000, -0.2000000000, -179.6000000000))]
    #[test_case((-0.0017453284, 0.9999923846, 0.0034906461, 451.0000000000, 0.9999984769, 0.0017453177, 0.0000060923, 265.4000000000, -0.0000000000, 0.0034906514, -0.9999939077, -242.9000000000, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (451.0000000000, 265.4000000000, -242.9000000000, 179.8000000000, -0.0000000000, 90.1000000000))]
    #[test_case((-0.0069812603, 0.9999741077, -0.0017452858, 450.9000000000, 0.9999756307, 0.0069812497, -0.0000121846, 62.5000000000, 0.0000000000, -0.0017453284, -0.9999984769, -242.5000000000, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (450.9000000000, 62.5000000000, -242.5000000000, -179.9000000000, 0.0000000000, 90.4000000000))]
    #[test_case((0.1357122651, 0.9906854758, -0.0111565722, 508.7000000000, 0.9907236966, -0.1356209430, 0.0085741896, -253.6000000000, 0.0069812603, -0.0122167031, -0.9999010022, -197.2000000000, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (508.7000000000, -253.6000000000, -197.2000000000, -179.3000000000, -0.4000000000, 82.2000000000))]
    #[test_case((-0.8156824504, -0.5743236360, -0.0693866068, -413.8635232282, 0.0004524620, 0.1193088953, -0.9928570807, 263.1863811434, 0.5784997281, -0.8098874913, -0.0970583124, 291.9820746748, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-413.8635232282, 263.1863811434, 291.9820746748, -96.8338333769, -35.3450905433, 179.9682178248))]
    #[test_case((-0.1500516217, 0.5455385155, -0.8245436550, -149.1668440821, 0.9459952737, -0.1631791693, -0.2801169415, 59.3543506644, -0.2873629291, -0.8220464019, -0.4915915582, 543.2838251943, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-149.1668440821, 59.3543506644, 543.2838251943, -120.8798246395, 16.7001441168, 99.0130388728))]
    #[test_case((0.5009445358, 0.6825401810, 0.5321592557, 31.0642288750, -0.8639774486, 0.3581642754, 0.3539227601, -26.7743169615, 0.0509660705, -0.6370692687, 0.7691197609, 37.9006405971, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (31.0642288750, -26.7743169615, 37.9006405971, -39.6352782596, -2.9214064154, -59.8943255222))]
    #[test_case((0.9089103503, -0.4069146210, 0.0911178708, 33.5138781088, -0.0854907806, 0.0320310501, 0.9958239494, 96.1164814542, -0.4081339260, -0.9129044326, -0.0056740956, -300.9032245106, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (33.5138781088, 96.1164814542, -300.9032245106, -90.3561134473, 24.0876647389, -5.3733485621))]
    #[test_case((-0.1653518888, 0.9687851465, -0.1847000075, 77.8658952620, 0.9532759246, 0.2050088803, 0.2218927005, 100.2923343848, 0.2528314941, -0.1393796933, -0.9574181619, -127.6541936118, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (77.8658952620, 100.2923343848, -127.6541936118, -171.7171410357, -14.6451288996, 99.8404139441))]
    #[test_case((-0.4118951499, 0.3279601759, -0.8501673415, -70.6389144769, -0.3403719856, -0.9208299694, -0.1903131075, 22.3217390074, -0.8452746873, 0.2109841002, 0.4909138545, 18.1798498130, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-70.6389144769, 22.3217390074, 18.1798498130, 23.2569815201, 57.7013782986, -140.4311743532))]
    #[test_case((-0.7479917678, -0.6191725989, -0.2390263753, 604.2872268462, -0.4057545090, 0.1415967662, 0.9029471935, -20.9333741686, -0.5252347986, 0.7723830970, -0.3571452893, 30.4479677368, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (604.2872268462, -20.9333741686, 30.4479677368, 114.8155367373, 31.6840511268, -151.5220050073))]
    #[test_case((0.4873440333, 0.6172398073, 0.6176656163, 77.4016768659, 0.3323406191, 0.5230063131, -0.7848656633, -28.4720503689, -0.8074933474, 0.5877749712, 0.0497501464, -19.7908480100, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (77.4016768659, -28.4720503689, -19.7908480100, 85.1619317507, 53.8517426985, 34.2917400153))]
    #[test_case((0.0833272687, 0.6836449300, 0.7250421891, 471.9320306822, -0.8519144952, -0.3286045615, 0.4077508246, -102.6726730677, 0.5170089546, -0.6516507130, 0.5550253049, 588.5120154719, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (471.9320306822, -102.6726730677, 588.5120154719, -49.5782300627, -31.1318310910, -84.4135682965))]
    #[test_case((-0.2510504585, 0.9157805028, -0.3135597837, -577.6445766741, -0.0345374583, -0.3322023607, -0.9425755967, -216.2517731052, -0.9673576543, -0.2258044778, 0.1150282859, 240.9460890915, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-577.6445766741, -216.2517731052, 240.9460890915, -63.0050045001, 75.3203125033, -172.1668871906))]
    #[test_case((0.1791056335, -0.5890186234, -0.7880217213, 411.6803610302, 0.6839325663, -0.5012295385, 0.5300992308, 331.0434986878, -0.7072180829, -0.6338974767, 0.3130759848, -361.9951518329, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (411.6803610302, 331.0434986878, -361.9951518329, -63.7156683382, 45.0090193165, 75.3251405039))]
    #[test_case((-0.7389487926, -0.6540675080, 0.1617107816, 5.4890201453, -0.6393453686, 0.7564348576, 0.1379992968, -26.5744088848, -0.2125845282, -0.0014146255, -0.9771416567, -99.6628429743, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (5.4890201453, -26.5744088848, -99.6628429743, -179.9170519309, 12.2738553488, -139.1333357015))]
    #[test_case((-0.2037395422, 0.6790363200, -0.7052658187, -1.7434467919, -0.8508250650, -0.4791894336, -0.2155787453, -14.7633944063, -0.4843417261, 0.5561359212, 0.6753709570, -79.9344894886, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-1.7434467919, -14.7633944063, -79.9344894886, 39.4698348110, 28.9693530939, -103.4665369616))]
    #[test_case((0.7230637290, 0.1432361537, -0.6757678952, -49.7118498703, 0.5101470443, 0.5488670300, 0.6621895322, 20.3765636776, 0.4657561992, -0.8235462268, 0.3237943410, 8.4422995188, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-49.7118498703, 20.3765636776, 8.4422995188, -68.5366961539, -27.7591729450, 35.2043221840))]
    #[test_case((0.9572590453, -0.2498925755, 0.1456324858, 59.5794643289, -0.2886479285, -0.7934076632, 0.5358979878, 6.7548483372, -0.0183709982, -0.5550297115, -0.8316276365, -88.8341095469, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (59.5794643289, 6.7548483372, -88.8341095469, -146.2807856754, 1.0526398756, -16.7799423973))]
    // Tuple shape (flattened 4x4 matrix) is dictated by the `test_case` rows above.
    #[allow(clippy::type_complexity)]
    fn xyzwpr_to_isometry(
        mf: (
            f64,
            f64,
            f64,
            f64,
            f64,
            f64,
            f64,
            f64,
            f64,
            f64,
            f64,
            f64,
            f64,
            f64,
            f64,
            f64,
        ),
        pf: (f64, f64, f64, f64, f64, f64),
    ) {
        let m = Matrix4::new(
            mf.0, mf.1, mf.2, mf.3, mf.4, mf.5, mf.6, mf.7, mf.8, mf.9, mf.10, mf.11, mf.12, mf.13,
            mf.14, mf.15,
        );
        let p = XyzWpr::new(pf.0, pf.1, pf.2, pf.3, pf.4, pf.5);
        let t = Iso3::from(&p);
        let m_ = t.to_matrix();
        assert_relative_eq!(m, m_, epsilon = 1e-8);
    }

    #[test]
    fn xyzwpr_to_isometry_single() {
        let x = XyzWpr::new(
            151.8000000000,
            677.1000000000,
            277.0000000000,
            119.2000000000,
            -0.2000000000,
            -179.6000000000,
        );

        let e = Matrix4::new(
            -0.9999695385116514,
            -0.0003588828397861686,
            -0.007797002751219406,
            151.8,
            -0.006981217765734895,
            0.48786904270864234,
            0.8728889160512124,
            677.1,
            0.003490651415223732,
            0.8729167591297927,
            -0.4878566869307391,
            277.0,
            0.0,
            0.0,
            0.0,
            1.0,
        );

        let m = Iso3::from(&x).to_matrix();

        assert_relative_eq!(m, e, epsilon = 1e-8);
    }

    #[test]
    fn isometry_to_xyzwpr_single() {
        let _x = XyzWpr::new(
            151.8000000000,
            677.1000000000,
            277.0000000000,
            119.2000000000,
            -0.2000000000,
            -179.6000000000,
        );

        let m = Matrix4::new(
            -0.9999695385116514,
            -0.0003588828397861686,
            -0.007797002751219406,
            151.8,
            -0.006981217765734895,
            0.48786904270864234,
            0.8728889160512124,
            677.1,
            0.003490651415223732,
            0.8729167591297927,
            -0.4878566869307391,
            277.0,
            0.0,
            0.0,
            0.0,
            1.0,
        );
        let iso: Iso3 = try_convert(m).unwrap();

        let p = XyzWpr::from(&iso);

        let pf = (
            151.8000000000,
            677.1000000000,
            277.0000000000,
            119.2000000000,
            -0.2000000000,
            -179.6000000000,
        );

        assert_relative_eq!(p.x, pf.0, epsilon = 1e-5);
        assert_relative_eq!(p.y, pf.1, epsilon = 1e-5);
        assert_relative_eq!(p.z, pf.2, epsilon = 1e-5);
        assert_relative_eq!(p.w, pf.3, epsilon = 1e-5);
        assert_relative_eq!(p.p, pf.4, epsilon = 1e-5);
        assert_relative_eq!(p.r, pf.5, epsilon = 1e-5);
    }
}