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

cartesian_tree/
pose.rs
