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    ) -> Pose
28    where
29        O: IntoOrientation,
30    {
31        Pose {
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    pub fn frame(&self) -> Option<Frame> {
56        self.parent.upgrade().map(|data| Frame { data })
57    }
58
59    /// Returns the transformation from this pose to its parent frame.
60    ///
61    /// # Returns
62    /// The transformation of the pose in its parent frame.
63    pub fn transformation(&self) -> Isometry3<f64> {
64        self.transform_to_parent
65    }
66
67    /// Updates the pose's transformation relative to its parent.
68    ///
69    /// # Arguments
70    /// - `position`: A 3D vector representing the new translational offset from the parent.
71    /// - `orientation`: An orientation convertible into a unit quaternion for new orientational offset from the parent.
72    ///
73    /// # Example
74    /// ```
75    /// use cartesian_tree::Frame;
76    /// use nalgebra::{Vector3, UnitQuaternion};
77    ///
78    /// let root = Frame::new_origin("root");
79    /// let mut pose = root.add_pose(Vector3::new(0.0, 0.0, 1.0), UnitQuaternion::identity());
80    /// pose.update(Vector3::new(1.0, 0.0, 0.0), UnitQuaternion::identity());
81    /// ```
82    pub fn update<O>(&mut self, position: Vector3<f64>, orientation: O)
83    where
84        O: IntoOrientation,
85    {
86        self.transform_to_parent =
87            Isometry3::from_parts(Translation3::from(position), orientation.into_orientation());
88    }
89
90    /// Transforms this pose into the coordinate system of the given target frame.
91    ///
92    /// # Arguments
93    /// * `target` - The frame to express this pose in.
94    ///
95    /// # Returns
96    /// - `Ok(Pose)` a new `Pose`, expressed in the `target` frame.
97    /// - `Err(String)` if the frame hierarchy cannot be resolved (due to dropped frames or no common ancestor).
98    ///
99    /// # Example
100    /// ```
101    /// use cartesian_tree::Frame;
102    /// use nalgebra::{Vector3, UnitQuaternion};
103    ///
104    /// let root = Frame::new_origin("root");
105    /// let pose = root.add_pose(Vector3::new(0.0, 0.0, 1.0), UnitQuaternion::identity());
106    /// let new_frame = root.add_child("child", Vector3::new(1.0, 0.0, 0.0), UnitQuaternion::identity()).unwrap();
107    /// let pose_in_new_frame = pose.in_frame(&new_frame);
108    /// ```
109    pub fn in_frame(&self, target: &Frame) -> Result<Pose, CartesianTreeError> {
110        let source_data = self
111            .parent
112            .upgrade()
113            .ok_or(CartesianTreeError::WeakUpgradeFailed())?;
114        let source = Frame { data: source_data };
115        let ancestor = source
116            .lca_with(target)
117            .ok_or(CartesianTreeError::NoCommonAncestor(
118                source.name(),
119                target.name(),
120            ))?;
121
122        // Transformation from source frame up to ancestor
123        let tf_up = source.walk_up_and_transform(&ancestor)? * self.transform_to_parent;
124
125        // Transformation from target frame up to ancestor (to be inverted)
126        let tf_down = target.walk_up_and_transform(&ancestor)?;
127
128        Ok(Pose {
129            parent: target.downgrade(),
130            transform_to_parent: tf_down.inverse() * tf_up,
131        })
132    }
133}
cartesian_tree/pose.rs

cartesian_tree/
pose.rs
