CartesianTree 0.3.0

Build hierarchical Cartesian coordinate systems to easily transform poses.
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
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110

use pyo3::prelude::*;

use crate::{
    Pose as RustPose,
    bindings::{
        PyFrame,
        lazy_access::{PyLazyRotation, PyLazyTranslation},
        utils::{PyIsometry, PyRotation, PyVector3},
    },
};

#[pyclass(name = "Pose", unsendable)]
#[derive(Clone)]
pub struct PyPose {
    pub(crate) rust_pose: RustPose,
}

#[pymethods]
impl PyPose {
    fn frame(&self) -> Option<PyFrame> {
        self.rust_pose
            .frame()
            .map(|frame| PyFrame { rust_frame: frame })
    }

    fn transformation(&self) -> (PyVector3, PyRotation) {
        let isometry = self.rust_pose.transformation();
        (
            PyVector3 {
                inner: isometry.translation.vector,
            },
            PyRotation {
                rust_rotation: isometry.rotation.into(),
            },
        )
    }

    #[getter]
    const fn position(&self) -> PyVector3 {
        PyVector3 {
            inner: self.rust_pose.position(),
        }
    }

    #[getter]
    fn orientation(&self) -> PyRotation {
        PyRotation {
            rust_rotation: self.rust_pose.orientation(),
        }
    }

    #[pyo3(signature = (position, orientation))]
    fn set(&mut self, position: PyVector3, orientation: PyRotation) {
        self.rust_pose
            .set(position.inner, orientation.rust_rotation);
    }

    #[pyo3(signature = (isometry))]
    fn apply_in_parent_frame(&mut self, isometry: PyIsometry) {
        self.rust_pose.apply_in_parent_frame(&isometry.inner);
    }

    #[pyo3(signature = (isometry))]
    fn apply_in_local_frame(&mut self, isometry: PyIsometry) {
        self.rust_pose.apply_in_local_frame(&isometry.inner);
    }

    #[pyo3(signature = (target_frame))]
    fn in_frame(&self, target_frame: &PyFrame) -> PyResult<Self> {
        let new_rust_pose = self.rust_pose.in_frame(&target_frame.rust_frame)?;
        Ok(Self {
            rust_pose: new_rust_pose,
        })
    }

    fn __add__(&self, other: PyLazyTranslation) -> Self {
        let new_rust_pose = &self.rust_pose + other.inner;
        Self {
            rust_pose: new_rust_pose,
        }
    }

    fn __sub__(&self, other: PyLazyTranslation) -> Self {
        let new_rust_pose = &self.rust_pose - other.inner;
        Self {
            rust_pose: new_rust_pose,
        }
    }

    fn __mul__(&self, other: PyLazyRotation) -> Self {
        let new_rust_pose = &self.rust_pose * other.inner;
        Self {
            rust_pose: new_rust_pose,
        }
    }

    fn __str__(&self) -> String {
        let isometry = self.rust_pose.transformation();
        let vector = isometry.translation.vector;
        let quaternion = isometry.rotation.coords;
        format!(
            "({:.2}, {:.2}, {:.2})({:.4}, {:.4}, {:.4}, {:.4})",
            vector.x, vector.y, vector.z, quaternion.x, quaternion.y, quaternion.z, quaternion.w,
        )
    }

    fn __repr__(&self) -> String {
        self.__str__()
    }
}