cartesian_tree/
pose.rs

1use crate::CartesianTreeError;
2use crate::frame::{Frame, FrameData};
3use crate::orientation::IntoOrientation;
4use crate::tree::Walking;
5use nalgebra::{Isometry3, Translation3, Vector3};
6use std::cell::RefCell;
7use std::rc::Weak;
8
9/// Use [`Frame::add_pose`] to create a new pose.
10#[derive(Clone, Debug)]
11pub struct Pose {
12    /// Reference to the parent frame.
13    parent: Weak<RefCell<FrameData>>,
14    /// Transformation from this frame to its parent frame.
15    transform_to_parent: Isometry3<f64>,
16}
17
18impl Pose {
19    /// Creates a new pose relative to a frame.
20    ///
21    /// This function is intended for internal use. To create a pose associated with a frame,
22    /// use [`Frame::add_pose`], which handles the association safely.
23    pub(crate) fn new<O>(
24        frame: Weak<RefCell<FrameData>>,
25        position: Vector3<f64>,
26        orientation: O,
27    ) -> Self
28    where
29        O: IntoOrientation,
30    {
31        Self {
32            parent: frame,
33            transform_to_parent: Isometry3::from_parts(
34                Translation3::from(position),
35                orientation.into_orientation(),
36            ),
37        }
38    }
39
40    /// Returns the parent frame of this pose.
41    ///
42    /// # Returns
43    /// `Some(Frame)` if the parent frame is still valid, or `None` if the frame
44    /// has been dropped or no longer exists.
45    ///
46    /// # Example
47    /// ```
48    /// use cartesian_tree::Frame;
49    /// use nalgebra::{Vector3, UnitQuaternion};
50    ///
51    /// let frame = Frame::new_origin("base");
52    /// let pose = frame.add_pose(Vector3::new(0.0, 0.0, 0.0), UnitQuaternion::identity());
53    /// assert_eq!(pose.frame().unwrap().name(), "base");
54    /// ```
55    #[must_use]
56    pub fn frame(&self) -> Option<Frame> {
57        self.parent.upgrade().map(|data| Frame { data })
58    }
59
60    /// Returns the transformation from this pose to its parent frame.
61    ///
62    /// # Returns
63    /// The transformation of the pose in its parent frame.
64    #[must_use]
65    pub const fn transformation(&self) -> Isometry3<f64> {
66        self.transform_to_parent
67    }
68
69    /// Updates the pose's transformation relative to its parent.
70    ///
71    /// # Arguments
72    /// - `position`: A 3D vector representing the new translational offset from the parent.
73    /// - `orientation`: An orientation convertible into a unit quaternion for new orientational offset from the parent.
74    ///
75    /// # Example
76    /// ```
77    /// use cartesian_tree::Frame;
78    /// use nalgebra::{Vector3, UnitQuaternion};
79    ///
80    /// let root = Frame::new_origin("root");
81    /// let mut pose = root.add_pose(Vector3::new(0.0, 0.0, 1.0), UnitQuaternion::identity());
82    /// pose.update(Vector3::new(1.0, 0.0, 0.0), UnitQuaternion::identity());
83    /// ```
84    pub fn update<O>(&mut self, position: Vector3<f64>, orientation: O)
85    where
86        O: IntoOrientation,
87    {
88        self.transform_to_parent =
89            Isometry3::from_parts(Translation3::from(position), orientation.into_orientation());
90    }
91
92    /// Transforms this pose into the coordinate system of the given target frame.
93    ///
94    /// # Arguments
95    /// * `target` - The frame to express this pose in.
96    ///
97    /// # Returns
98    /// A new `Pose`, expressed in the `target` frame.
99    ///
100    /// # Errors
101    /// Returns a [`CartesianTreeError`] if:
102    /// - The frame hierarchy cannot be resolved (e.g., due to dropped frames).
103    /// - There is no common ancestor between `self` and `target`.
104    ///
105    /// # Example
106    /// ```
107    /// use cartesian_tree::Frame;
108    /// use nalgebra::{Vector3, UnitQuaternion};
109    ///
110    /// let root = Frame::new_origin("root");
111    /// let pose = root.add_pose(Vector3::new(0.0, 0.0, 1.0), UnitQuaternion::identity());
112    /// let new_frame = root.add_child("child", Vector3::new(1.0, 0.0, 0.0), UnitQuaternion::identity()).unwrap();
113    /// let pose_in_new_frame = pose.in_frame(&new_frame);
114    /// ```
115    pub fn in_frame(&self, target: &Frame) -> Result<Self, CartesianTreeError> {
116        let source_data = self
117            .parent
118            .upgrade()
119            .ok_or(CartesianTreeError::WeakUpgradeFailed())?;
120        let source = Frame { data: source_data };
121        let ancestor = source
122            .lca_with(target)
123            .ok_or_else(|| CartesianTreeError::NoCommonAncestor(source.name(), target.name()))?;
124
125        // Transformation from source frame up to ancestor
126        let tf_up = source.walk_up_and_transform(&ancestor)? * self.transform_to_parent;
127
128        // Transformation from target frame up to ancestor (to be inverted)
129        let tf_down = target.walk_up_and_transform(&ancestor)?;
130
131        Ok(Self {
132            parent: target.downgrade(),
133            transform_to_parent: tf_down.inverse() * tf_up,
134        })
135    }
136}
cartesian_tree/pose.rs

cartesian_tree/
pose.rs
