cartesian_tree/
frame.rs

1use crate::CartesianTreeError;
2use crate::Pose;
3use crate::lazy_access::LazyRotation;
4use crate::lazy_access::LazyTranslation;
5use crate::rotation::Rotation;
6use crate::tree::Walking;
7use crate::tree::{HasChildren, HasParent, NodeEquality};
8
9use nalgebra::UnitQuaternion;
10use nalgebra::{Isometry3, Translation3, Vector3};
11use std::cell::RefCell;
12use std::ops::Add;
13use std::ops::Mul;
14use std::ops::Sub;
15use std::rc::{Rc, Weak};
16
17use serde::{Deserialize, Serialize};
18use serde_json;
19use uuid::Uuid;
20
21/// Represents a coordinate frame in a Cartesian tree structure.
22///
23/// Each frame can have one parent and multiple children. The frame stores its
24/// transformation (position and orientation) relative to its parent.
25///
26/// Root frames (created via `Frame::new_origin`) have no parent and use the identity transform.
27#[derive(Clone, Debug)]
28pub struct Frame {
29    pub(crate) data: Rc<RefCell<FrameData>>,
30}
31
32#[derive(Debug)]
33pub(crate) struct FrameData {
34    /// The name of the frame (must be unique among siblings).
35    pub(crate) name: String,
36    /// Reference to the parent frame.
37    parent: Option<Weak<RefCell<FrameData>>>,
38    /// Transformation from this frame to its parent frame.
39    transform_to_parent: Isometry3<f64>,
40    /// Child frames directly connected to this frame.
41    children: Vec<Frame>,
42}
43
44#[derive(Serialize, Deserialize, Debug, Clone)]
45struct SerialFrame {
46    name: String,
47    position: Vector3<f64>,
48    orientation: UnitQuaternion<f64>,
49    children: Vec<SerialFrame>,
50}
51
52impl Frame {
53    /// Creates a new root frame (origin) with the given name.
54    ///
55    /// The origin has no parent and uses the identity transform.
56    /// # Arguments
57    /// - `name`: The name of the root frame.
58    ///
59    /// # Example
60    /// ```
61    /// use cartesian_tree::Frame;
62    ///
63    /// let origin = Frame::new_origin("world");
64    /// ```
65    pub fn new_origin(name: impl Into<String>) -> Self {
66        Self {
67            data: Rc::new(RefCell::new(FrameData {
68                name: name.into(),
69                parent: None,
70                children: Vec::new(),
71                transform_to_parent: Isometry3::identity(),
72            })),
73        }
74    }
75
76    pub(crate) fn borrow(&self) -> std::cell::Ref<'_, FrameData> {
77        self.data.borrow()
78    }
79
80    fn borrow_mut(&self) -> std::cell::RefMut<'_, FrameData> {
81        self.data.borrow_mut()
82    }
83
84    pub(crate) fn downgrade(&self) -> Weak<RefCell<FrameData>> {
85        Rc::downgrade(&self.data)
86    }
87
88    pub(crate) fn walk_up_and_transform(
89        &self,
90        target: &Self,
91    ) -> Result<Isometry3<f64>, CartesianTreeError> {
92        let mut transform = Isometry3::identity();
93        let mut current = self.clone();
94
95        while !current.is_same(target) {
96            let transform_to_its_parent = {
97                // Scope borrow
98                let current_data = current.borrow();
99
100                // If current frame is root and not target, then target is not an ancestor.
101                if current_data.parent.is_none() {
102                    return Err(CartesianTreeError::IsNoAncestor(target.name(), self.name()));
103                }
104                current_data.transform_to_parent
105            };
106
107            transform = transform_to_its_parent * transform;
108
109            let parent_frame_opt = current.parent();
110            current = parent_frame_opt
111                .ok_or_else(|| CartesianTreeError::IsNoAncestor(target.name(), self.name()))?;
112        }
113
114        Ok(transform)
115    }
116
117    /// Returns the name of the frame.
118    #[must_use]
119    pub fn name(&self) -> String {
120        self.borrow().name.clone()
121    }
122
123    /// Returns the transformation from this frame to its parent frame.
124    ///
125    /// # Returns
126    /// - The isometry from this frame to its parent frame.
127    ///
128    /// # Errors
129    /// Returns a [`CartesianTreeError`] if:
130    /// - The frame has no parent.
131    pub fn transformation(&self) -> Result<Isometry3<f64>, CartesianTreeError> {
132        if self.parent().is_none() {
133            return Err(CartesianTreeError::RootHasNoParent(self.name()));
134        }
135        Ok(self.borrow().transform_to_parent)
136    }
137
138    /// Returns the position of this frame relative to its parent frame.
139    ///
140    /// # Returns
141    /// The position of the frame in its parent frame.
142    #[must_use]
143    pub fn position(&self) -> Vector3<f64> {
144        self.borrow().transform_to_parent.translation.vector
145    }
146
147    /// Returns the orientation of this frame relative to its parent frame.
148    ///
149    /// # Returns
150    /// The orientation of the frame in its parent frame.
151    #[must_use]
152    pub fn orientation(&self) -> Rotation {
153        self.borrow().transform_to_parent.rotation.into()
154    }
155
156    /// Sets the frame's transformation relative to its parent.
157    ///
158    /// This method modifies the frame's position and orientation relative to its parent frame.
159    /// It fails if the frame is a root frame (i.e., has no parent).
160    ///
161    /// # Arguments
162    /// - `position`: A 3D vector representing the new translational offset from the parent.
163    /// - `orientation`: An orientation convertible into a unit quaternion for new orientational offset from the parent.
164    ///
165    /// # Returns
166    /// - `Ok(())` if the transformation was updated successfully.
167    ///
168    /// # Errors
169    /// Returns a [`CartesianTreeError`] if:
170    /// - The frame has no parent (i.e., the root frame).
171    ///
172    /// # Example
173    /// ```
174    /// use cartesian_tree::Frame;
175    /// use nalgebra::{Vector3, UnitQuaternion};
176    ///
177    /// let root = Frame::new_origin("root");
178    /// let child = root
179    ///     .add_child("camera", Vector3::new(0.0, 0.0, 1.0), UnitQuaternion::identity())
180    ///     .unwrap();
181    /// child.set(Vector3::new(1.0, 0.0, 0.0), UnitQuaternion::identity())
182    ///     .unwrap();
183    /// ```
184    pub fn set(
185        &self,
186        position: Vector3<f64>,
187        orientation: impl Into<Rotation>,
188    ) -> Result<(), CartesianTreeError> {
189        if self.parent().is_none() {
190            return Err(CartesianTreeError::CannotUpdateRootTransform(self.name()));
191        }
192        self.borrow_mut().transform_to_parent = Isometry3::from_parts(
193            Translation3::from(position),
194            orientation.into().as_quaternion(),
195        );
196        Ok(())
197    }
198
199    /// Applies the provided isometry interpreted in the parent frame to this frame.
200    ///
201    /// This method modifies the frame's position and orientation relative to its current position and orientation.
202    /// It fails if the frame is a root frame (i.e., has no parent).
203    ///
204    /// # Arguments
205    /// - `isometry`: The isometry (describing a motion in the parent frame coordinates) to apply to the current transformation.
206    ///
207    /// # Returns
208    /// - `Ok(())` if the transformation was updated successfully.
209    ///
210    /// # Errors
211    /// Returns a [`CartesianTreeError`] if:
212    /// - The frame has no parent (i.e., the root frame).
213    ///
214    /// # Example
215    /// ```
216    /// use cartesian_tree::Frame;
217    /// use nalgebra::{Isometry3, Translation3, Vector3, UnitQuaternion};
218    ///
219    /// let root = Frame::new_origin("root");
220    /// let child = root
221    ///     .add_child("camera", Vector3::new(1.0, 0.0, 1.0), UnitQuaternion::identity())
222    ///     .unwrap();
223    /// child.apply_in_parent_frame(&Isometry3::from_parts(Translation3::new(1.0, 0.0, 0.0), UnitQuaternion::identity()))
224    ///     .unwrap();
225    ///
226    /// ```
227    pub fn apply_in_parent_frame(
228        &self,
229        isometry: &Isometry3<f64>,
230    ) -> Result<(), CartesianTreeError> {
231        if self.parent().is_none() {
232            return Err(CartesianTreeError::CannotUpdateRootTransform(self.name()));
233        }
234        let mut borrow = self.borrow_mut();
235        borrow.transform_to_parent = isometry * borrow.transform_to_parent;
236        Ok(())
237    }
238
239    /// Applies the provided isometry interpreted in this frame to this frame.
240    ///
241    /// This method modifies the frame's position and orientation relative to its current position and orientation.
242    /// It fails if the frame is a root frame (i.e., has no parent).
243    ///
244    /// # Arguments
245    /// - `isometry`: The isometry (describing a motion in this frame) to apply to the current transformation.
246    ///
247    /// # Returns
248    /// - `Ok(())` if the transformation was updated successfully.
249    ///
250    /// # Errors
251    /// Returns a [`CartesianTreeError`] if:
252    /// - The frame has no parent (i.e., the root frame).
253    ///
254    /// # Example
255    /// ```
256    /// use cartesian_tree::Frame;
257    /// use nalgebra::{Isometry3, Translation3, Vector3, UnitQuaternion};
258    ///
259    /// let root = Frame::new_origin("root");
260    /// let child = root
261    ///     .add_child("camera", Vector3::zeros(), UnitQuaternion::from_euler_angles(0.0, 0.0, std::f64::consts::FRAC_PI_2))
262    ///     .unwrap();
263    /// child.apply_in_local_frame(&Isometry3::from_parts(Translation3::new(1.0, 0.0, 0.0), UnitQuaternion::identity()))
264    ///     .unwrap();
265    ///
266    /// ```
267    pub fn apply_in_local_frame(
268        &self,
269        isometry: &Isometry3<f64>,
270    ) -> Result<(), CartesianTreeError> {
271        if self.parent().is_none() {
272            return Err(CartesianTreeError::CannotUpdateRootTransform(self.name()));
273        }
274        let mut borrow = self.borrow_mut();
275        borrow.transform_to_parent *= isometry;
276        Ok(())
277    }
278
279    /// Adds a new child frame to the current frame.
280    ///
281    /// The child is positioned and oriented relative to this frame.
282    ///
283    /// Returns an error if a child with the same name already exists.
284    ///
285    /// # Arguments
286    /// - `name`: The name of the new child frame.
287    /// - `position`: A 3D vector representing the translational offset from the parent.
288    /// - `orientation`: An orientation convertible into a unit quaternion.
289    ///
290    /// # Returns
291    /// The newly added child frame.
292    ///
293    /// # Errors
294    /// Returns a [`CartesianTreeError`] if:
295    /// - A child with the same name already exists.
296    ///
297    /// # Example
298    /// ```
299    /// use cartesian_tree::Frame;
300    /// use nalgebra::{Vector3, UnitQuaternion};
301    ///
302    /// let root = Frame::new_origin("base");
303    /// let child = root
304    ///     .add_child("camera", Vector3::new(0.0, 0.0, 1.0), UnitQuaternion::identity())
305    ///     .unwrap();
306    /// ```
307    pub fn add_child(
308        &self,
309        name: impl Into<String>,
310        position: Vector3<f64>,
311        orientation: impl Into<Rotation>,
312    ) -> Result<Self, CartesianTreeError> {
313        let child_name = name.into();
314        {
315            let frame = self.borrow();
316            if frame
317                .children
318                .iter()
319                .any(|child| child.borrow().name == child_name)
320            {
321                return Err(CartesianTreeError::ChildNameConflict(
322                    child_name,
323                    self.name(),
324                ));
325            }
326        }
327        let transform = Isometry3::from_parts(
328            Translation3::from(position),
329            orientation.into().as_quaternion(),
330        );
331
332        let child = Self {
333            data: Rc::new(RefCell::new(FrameData {
334                name: child_name,
335                parent: Some(Rc::downgrade(&self.data)),
336                children: Vec::new(),
337                transform_to_parent: transform,
338            })),
339        };
340
341        self.borrow_mut().children.push(child.clone());
342        Ok(child)
343    }
344
345    /// Adds a new child frame calibrated such that a reference pose, when expressed in the new frame,
346    /// matches the desired position and orientation.
347    ///
348    /// # Arguments
349    /// - `name`: The name of the new child frame.
350    /// - `desired_position`: The desired position of the reference pose in the new frame.
351    /// - `desired_orientation`: The desired orientation of the reference pose in the new frame.
352    /// - `reference_pose`: The existing pose (in some frame A) used as the calibration reference.
353    ///
354    /// # Returns
355    /// - The new child frame if successful.
356    ///
357    /// # Errors
358    /// Returns a [`CartesianTreeError`] if:
359    /// - The reference frame is invalid.
360    /// - No common ancestor exists.
361    /// - A child with the same name already exists.
362    ///
363    /// # Example
364    /// ```
365    /// use cartesian_tree::Frame;
366    /// use nalgebra::{Vector3, UnitQuaternion};
367    ///
368    /// let root = Frame::new_origin("root");
369    /// let reference_pose = root.add_pose(Vector3::new(1.0, 0.0, 0.0), UnitQuaternion::identity());
370    /// let calibrated_child = root.calibrate_child(
371    ///     "calibrated",
372    ///     Vector3::zeros(),
373    ///     UnitQuaternion::identity(),
374    ///     &reference_pose,
375    /// ).unwrap();
376    /// ```
377    pub fn calibrate_child(
378        &self,
379        name: impl Into<String>,
380        desired_position: Vector3<f64>,
381        desired_orientation: impl Into<Rotation>,
382        reference_pose: &Pose,
383    ) -> Result<Self, CartesianTreeError> {
384        let reference_frame = reference_pose.frame().ok_or_else(|| {
385            CartesianTreeError::FrameDropped("Reference pose frame has been dropped".to_string())
386        })?;
387
388        let ancestor = self.lca_with(&reference_frame).ok_or_else(|| {
389            CartesianTreeError::NoCommonAncestor(self.name(), reference_frame.name())
390        })?;
391
392        let t_reference_to_ancestor = reference_frame.walk_up_and_transform(&ancestor)?;
393        let t_pose_to_reference = reference_pose.transformation();
394        let t_pose_to_ancestor = t_reference_to_ancestor * t_pose_to_reference;
395
396        let t_parent_to_ancestor = self.walk_up_and_transform(&ancestor)?;
397        let t_ancestor_to_parent = t_parent_to_ancestor.inverse();
398
399        let desired_pose = Isometry3::from_parts(
400            Translation3::from(desired_position),
401            desired_orientation.into().as_quaternion(),
402        );
403
404        let t_calibrated_to_parent =
405            t_pose_to_ancestor * desired_pose.inverse() * t_ancestor_to_parent;
406
407        self.add_child(
408            name,
409            t_calibrated_to_parent.translation.vector,
410            t_calibrated_to_parent.rotation,
411        )
412    }
413
414    /// Adds a pose to the current frame.
415    ///
416    /// # Arguments
417    /// - `position`: The translational part of the pose.
418    /// - `orientation`: The orientational part of the pose.
419    ///
420    /// # Returns
421    /// - The newly added pose.
422    ///
423    /// # Example
424    /// ```
425    /// use cartesian_tree::Frame;
426    /// use nalgebra::{Vector3, UnitQuaternion};
427    ///
428    /// let frame = Frame::new_origin("base");
429    /// let pose = frame.add_pose(Vector3::new(0.5, 0.0, 0.0), UnitQuaternion::identity());
430    /// ```
431    pub fn add_pose(&self, position: Vector3<f64>, orientation: impl Into<Rotation>) -> Pose {
432        Pose::new(self.downgrade(), position, orientation)
433    }
434
435    /// Serializes the frame tree to a JSON string.
436    ///
437    /// This recursively serializes the hierarchy starting from this frame (ideally the root).
438    /// Transforms for root frames are set to identity.
439    ///
440    /// # Returns
441    /// The serialized tree as a JSON string.
442    ///
443    /// # Errors
444    /// Returns a [`CartesianTreeError`] if:
445    /// - On deserialization failure.
446    pub fn to_json(&self) -> Result<String, CartesianTreeError> {
447        let serial = self.to_serial();
448        Ok(serde_json::to_string_pretty(&serial)?)
449    }
450
451    /// Helper function to convert the frame and its children recursively into a serializable structure.
452    ///
453    /// This is used internally for JSON serialization.
454    fn to_serial(&self) -> SerialFrame {
455        let (position, orientation) = if self.parent().is_some() {
456            let iso = self
457                .transformation()
458                .unwrap_or_else(|_| Isometry3::identity());
459            (iso.translation.vector, iso.rotation)
460        } else {
461            (Vector3::zeros(), UnitQuaternion::identity())
462        };
463
464        SerialFrame {
465            name: self.name(),
466            position,
467            orientation,
468            children: self.children().into_iter().map(|c| c.to_serial()).collect(),
469        }
470    }
471
472    /// Applies a JSON config to this frame tree by updating matching transforms.
473    ///
474    /// Deserializes the JSON to a temporary structure, then recursively updates transforms
475    /// where names match (partial apply; ignores unmatched frames in config).
476    /// Skips updating root frames (identity assumed) - assumes this frame is the root.
477    ///
478    /// # Arguments
479    /// - `json`: The JSON string to apply.
480    ///
481    /// # Returns
482    /// `Ok(())` if applied successfully (even if partial).
483    ///
484    /// # Errors
485    /// Returns a [`CartesianTreeError`] if:
486    /// - On deserialization failure.
487    /// - The frame names do not match at the root.
488    ///
489    pub fn apply_config(&self, json: &str) -> Result<(), CartesianTreeError> {
490        let serial: SerialFrame = serde_json::from_str(json)?;
491        self.apply_serial(&serial)
492    }
493
494    fn apply_serial(&self, serial: &SerialFrame) -> Result<(), CartesianTreeError> {
495        if self.name() != serial.name {
496            return Err(CartesianTreeError::Mismatch(format!(
497                "Frame names do not match: {} vs {}",
498                self.name(),
499                serial.name
500            )));
501        }
502
503        // only update if frame has parent
504        if self.parent().is_some() {
505            self.set(serial.position, serial.orientation)?;
506        }
507
508        for potential_child in &serial.children {
509            if let Some(child) = self
510                .children()
511                .into_iter()
512                .find(|c| c.name() == potential_child.name)
513            {
514                child.apply_serial(potential_child)?;
515            }
516        }
517
518        Ok(())
519    }
520}
521
522impl Add<LazyTranslation> for &Frame {
523    type Output = Frame;
524
525    fn add(self, rhs: LazyTranslation) -> Self::Output {
526        let auto_name = Uuid::new_v4().to_string();
527        let current_position = self.borrow().transform_to_parent.translation.vector;
528        let current_orientation = self.borrow().transform_to_parent.rotation;
529        let child = self
530            .add_child(auto_name, current_position, current_orientation)
531            .unwrap();
532        child.apply_in_parent_frame(&rhs.inner).unwrap();
533        child // Not sure yet what to do with errors
534    }
535}
536
537impl Sub<LazyTranslation> for &Frame {
538    type Output = Frame;
539
540    fn sub(self, rhs: LazyTranslation) -> Self::Output {
541        let auto_name = Uuid::new_v4().to_string();
542        let current_position = self.borrow().transform_to_parent.translation.vector;
543        let current_orientation = self.borrow().transform_to_parent.rotation;
544        let child = self
545            .add_child(auto_name, current_position, current_orientation)
546            .unwrap();
547        child.apply_in_parent_frame(&rhs.inner.inverse()).unwrap();
548        child // Not sure yet what to do with errors
549    }
550}
551
552impl Mul<LazyRotation> for &Frame {
553    type Output = Frame;
554
555    fn mul(self, rhs: LazyRotation) -> Self::Output {
556        let auto_name = Uuid::new_v4().to_string();
557        let current_position = self.borrow().transform_to_parent.translation.vector;
558        let current_orientation = self.borrow().transform_to_parent.rotation;
559        let child = self
560            .add_child(auto_name, current_position, current_orientation)
561            .unwrap();
562        child.apply_in_local_frame(&rhs.inner).unwrap();
563        child // Not sure yet what to do with errors
564    }
565}
566
567impl HasParent for Frame {
568    type Node = Self;
569
570    fn parent(&self) -> Option<Self::Node> {
571        self.borrow()
572            .parent
573            .clone()
574            .and_then(|data_weak| data_weak.upgrade().map(|data_rc| Self { data: data_rc }))
575    }
576}
577
578impl NodeEquality for Frame {
579    fn is_same(&self, other: &Self) -> bool {
580        Rc::ptr_eq(&self.data, &other.data)
581    }
582}
583
584impl HasChildren for Frame {
585    type Node = Self;
586    fn children(&self) -> Vec<Self> {
587        self.borrow().children.clone()
588    }
589}
590
591#[cfg(test)]
592mod tests {
593    use crate::lazy_access::{rz, y, z};
594
595    use super::*;
596    use approx::assert_relative_eq;
597    use nalgebra::{UnitQuaternion, Vector3};
598
599    #[test]
600    fn create_origin_frame() {
601        let root = Frame::new_origin("world");
602        let root_borrow = root.borrow();
603        assert_eq!(root_borrow.name, "world");
604        assert!(root_borrow.parent.is_none());
605        assert_eq!(root_borrow.children.len(), 0);
606    }
607
608    #[test]
609    fn add_child_frame_with_quaternion() {
610        let root = Frame::new_origin("world");
611        let child = root
612            .add_child(
613                "dummy",
614                Vector3::new(1.0, 0.0, 0.0),
615                UnitQuaternion::identity(),
616            )
617            .unwrap();
618
619        let root_borrow = root.borrow();
620        assert_eq!(root_borrow.children.len(), 1);
621
622        let child_borrow = child.borrow();
623        assert_eq!(child_borrow.name, "dummy");
624        assert!(child_borrow.parent.is_some());
625
626        let parent_name = child_borrow
627            .parent
628            .as_ref()
629            .unwrap()
630            .upgrade()
631            .unwrap()
632            .borrow()
633            .name
634            .clone();
635        assert_eq!(parent_name, "world");
636    }
637
638    #[test]
639    fn add_child_frame_with_rpy() {
640        let root = Frame::new_origin("world");
641        let child = root
642            .add_child(
643                "dummy",
644                Vector3::new(0.0, 1.0, 0.0),
645                Rotation::from_rpy(0.0, 0.0, std::f64::consts::FRAC_PI_2),
646            )
647            .unwrap();
648
649        let child_borrow = child.borrow();
650        assert_eq!(child_borrow.name, "dummy");
651
652        let rotation = child_borrow.transform_to_parent.rotation;
653        let expected = UnitQuaternion::from_euler_angles(0.0, 0.0, std::f64::consts::FRAC_PI_2);
654        assert!((rotation.angle() - expected.angle()).abs() < 1e-10);
655    }
656
657    #[test]
658    fn test_child_frame_transform_to_parent() {
659        let root = Frame::new_origin("world");
660        let child = root
661            .add_child(
662                "dummy",
663                Vector3::new(0.0, 0.0, 1.0),
664                UnitQuaternion::identity(),
665            )
666            .unwrap();
667
668        let transform = child.transformation().unwrap();
669        assert_eq!(transform.translation.vector, Vector3::new(0.0, 0.0, 1.0));
670        assert_eq!(transform.rotation, UnitQuaternion::identity());
671
672        assert_eq!(child.position(), Vector3::new(0.0, 0.0, 1.0));
673        assert_eq!(
674            child.orientation().as_quaternion(),
675            UnitQuaternion::identity()
676        );
677    }
678
679    #[test]
680    fn multiple_child_frames() {
681        let root = Frame::new_origin("world");
682
683        let a = root
684            .add_child("a", Vector3::new(1.0, 0.0, 0.0), UnitQuaternion::identity())
685            .unwrap();
686        let b = root
687            .add_child("b", Vector3::new(0.0, 1.0, 0.0), UnitQuaternion::identity())
688            .unwrap();
689
690        let root_borrow = root.borrow();
691        assert_eq!(root_borrow.children.len(), 2);
692
693        let a_borrow = a.borrow();
694        let b_borrow = b.borrow();
695
696        assert_eq!(
697            a_borrow
698                .parent
699                .as_ref()
700                .unwrap()
701                .upgrade()
702                .unwrap()
703                .borrow()
704                .name,
705            "world"
706        );
707        assert_eq!(
708            b_borrow
709                .parent
710                .as_ref()
711                .unwrap()
712                .upgrade()
713                .unwrap()
714                .borrow()
715                .name,
716            "world"
717        );
718    }
719
720    #[test]
721    fn reject_duplicate_child_name() {
722        let root = Frame::new_origin("world");
723
724        let _ = root
725            .add_child(
726                "duplicate",
727                Vector3::new(1.0, 0.0, 0.0),
728                UnitQuaternion::identity(),
729            )
730            .unwrap();
731
732        let result = root.add_child(
733            "duplicate",
734            Vector3::new(2.0, 0.0, 0.0),
735            UnitQuaternion::identity(),
736        );
737        assert!(result.is_err());
738    }
739
740    #[test]
741    #[should_panic(expected = "already borrowed")]
742    fn test_borrow_conflict() {
743        let frame = Frame::new_origin("root");
744        let _borrow = frame.borrow(); // Immutable borrow
745        frame.borrow_mut(); // Should panic
746    }
747
748    #[test]
749    fn test_add_pose_to_frame() {
750        let frame = Frame::new_origin("dummy");
751        let pose = frame.add_pose(Vector3::new(1.0, 2.0, 3.0), UnitQuaternion::identity());
752
753        assert_eq!(pose.frame().unwrap().name(), "dummy");
754    }
755
756    #[test]
757    fn test_set_transform() {
758        let root = Frame::new_origin("root");
759        let child = root
760            .add_child(
761                "dummy",
762                Vector3::new(0.0, 0.0, 1.0),
763                UnitQuaternion::identity(),
764            )
765            .unwrap();
766        child
767            .set(Vector3::new(1.0, 0.0, 0.0), UnitQuaternion::identity())
768            .unwrap();
769        assert_eq!(
770            child.transformation().unwrap().translation.vector,
771            Vector3::new(1.0, 0.0, 0.0)
772        );
773
774        // Test root frame error
775        assert!(
776            root.set(Vector3::new(1.0, 0.0, 0.0), UnitQuaternion::identity())
777                .is_err()
778        );
779    }
780
781    #[test]
782    fn test_apply_in_parent_frame() {
783        let root = Frame::new_origin("root");
784        let child = root
785            .add_child(
786                "dummy",
787                Vector3::new(1.0, 0.0, 1.0),
788                UnitQuaternion::identity(),
789            )
790            .unwrap();
791        child
792            .apply_in_parent_frame(&Isometry3::from_parts(
793                Translation3::identity(),
794                UnitQuaternion::from_euler_angles(0.0, 0.0, std::f64::consts::FRAC_PI_2),
795            ))
796            .unwrap();
797
798        assert_relative_eq!(
799            child.transformation().unwrap().translation.vector,
800            Vector3::new(0.0, 1.0, 1.0),
801            epsilon = 1e-10
802        );
803
804        child
805            .apply_in_parent_frame(&Isometry3::from_parts(
806                Translation3::new(1.0, 0.0, 1.0),
807                UnitQuaternion::identity(),
808            ))
809            .unwrap();
810        assert_relative_eq!(
811            child.transformation().unwrap().translation.vector,
812            Vector3::new(1.0, 1.0, 2.0),
813            epsilon = 1e-10
814        );
815
816        // Test root frame error
817        assert!(
818            root.set(Vector3::new(1.0, 0.0, 0.0), UnitQuaternion::identity())
819                .is_err()
820        );
821    }
822
823    #[test]
824    fn test_apply_in_local_frame() {
825        let root = Frame::new_origin("root");
826        let child = root
827            .add_child(
828                "dummy",
829                Vector3::zeros(),
830                UnitQuaternion::from_euler_angles(0.0, 0.0, std::f64::consts::FRAC_PI_2),
831            )
832            .unwrap();
833
834        child
835            .apply_in_local_frame(&Isometry3::from_parts(
836                Translation3::new(1.0, 0.0, 0.0),
837                UnitQuaternion::identity(),
838            ))
839            .unwrap();
840
841        assert_relative_eq!(
842            child.transformation().unwrap().translation.vector,
843            Vector3::new(0.0, 1.0, 0.0),
844            epsilon = 1e-10
845        );
846
847        child
848            .apply_in_local_frame(&Isometry3::from_parts(
849                Translation3::identity(),
850                UnitQuaternion::from_euler_angles(0.0, 0.0, std::f64::consts::FRAC_PI_2),
851            ))
852            .unwrap();
853        assert_relative_eq!(
854            child.transformation().unwrap().translation.vector,
855            Vector3::new(0.0, 1.0, 0.0),
856            epsilon = 1e-10
857        );
858
859        let (roll, pitch, yaw) = child.transformation().unwrap().rotation.euler_angles();
860        assert_relative_eq!(roll, 0.0, epsilon = 1e-10);
861        assert_relative_eq!(pitch, 0.0, epsilon = 1e-10);
862        assert_relative_eq!(yaw, std::f64::consts::PI, epsilon = 1e-10);
863
864        // Test root frame error
865        assert!(
866            root.set(Vector3::new(1.0, 0.0, 0.0), UnitQuaternion::identity())
867                .is_err()
868        );
869    }
870
871    #[test]
872    fn test_pose_apply_in_parent_frame() {
873        let root = Frame::new_origin("root");
874        let mut pose = root.add_pose(Vector3::new(1.0, 0.0, 1.0), UnitQuaternion::identity());
875
876        pose.apply_in_parent_frame(&Isometry3::from_parts(
877            Translation3::identity(),
878            UnitQuaternion::from_euler_angles(0.0, 0.0, std::f64::consts::FRAC_PI_2),
879        ));
880
881        assert_relative_eq!(
882            pose.transformation().translation.vector,
883            Vector3::new(0.0, 1.0, 1.0),
884            epsilon = 1e-10
885        );
886
887        pose.apply_in_parent_frame(&Isometry3::from_parts(
888            Translation3::new(1.0, 0.0, 1.0),
889            UnitQuaternion::identity(),
890        ));
891        assert_relative_eq!(
892            pose.transformation().translation.vector,
893            Vector3::new(1.0, 1.0, 2.0),
894            epsilon = 1e-10
895        );
896    }
897
898    #[test]
899    fn test_pose_apply_in_local_frame() {
900        let root = Frame::new_origin("root");
901        let mut pose = root.add_pose(
902            Vector3::zeros(),
903            UnitQuaternion::from_euler_angles(0.0, 0.0, std::f64::consts::FRAC_PI_2),
904        );
905
906        pose.apply_in_local_frame(&Isometry3::from_parts(
907            Translation3::new(1.0, 0.0, 0.0),
908            UnitQuaternion::identity(),
909        ));
910
911        assert_relative_eq!(
912            pose.transformation().translation.vector,
913            Vector3::new(0.0, 1.0, 0.0),
914            epsilon = 1e-10
915        );
916
917        pose.apply_in_local_frame(&Isometry3::from_parts(
918            Translation3::identity(),
919            UnitQuaternion::from_euler_angles(0.0, 0.0, std::f64::consts::FRAC_PI_2),
920        ));
921        assert_relative_eq!(
922            pose.transformation().translation.vector,
923            Vector3::new(0.0, 1.0, 0.0),
924            epsilon = 1e-10
925        );
926
927        let (roll, pitch, yaw) = pose.transformation().rotation.euler_angles();
928        assert_relative_eq!(roll, 0.0, epsilon = 1e-10);
929        assert_relative_eq!(pitch, 0.0, epsilon = 1e-10);
930        assert_relative_eq!(yaw, std::f64::consts::PI, epsilon = 1e-10);
931
932        // Test root frame error
933        assert!(
934            root.set(Vector3::new(1.0, 0.0, 0.0), UnitQuaternion::identity())
935                .is_err()
936        );
937    }
938
939    #[test]
940    fn test_pose_transform_to_parent() {
941        let root = Frame::new_origin("root");
942        let pose = root.add_pose(Vector3::new(1.0, 2.0, 3.0), UnitQuaternion::identity());
943
944        let transformation = pose.transformation();
945        assert_eq!(
946            transformation.translation.vector,
947            Vector3::new(1.0, 2.0, 3.0)
948        );
949        assert_eq!(transformation.rotation, UnitQuaternion::identity());
950
951        assert_eq!(pose.position(), Vector3::new(1.0, 2.0, 3.0));
952        assert_eq!(
953            pose.orientation().as_quaternion(),
954            UnitQuaternion::identity()
955        );
956    }
957
958    #[test]
959    fn test_pose_transformation_between_frames() {
960        let root = Frame::new_origin("root");
961
962        let f1 = root
963            .add_child(
964                "f1",
965                Vector3::new(1.0, 0.0, 0.0),
966                UnitQuaternion::identity(),
967            )
968            .unwrap();
969
970        let f2 = f1
971            .add_child(
972                "f2",
973                Vector3::new(0.0, 2.0, 0.0),
974                UnitQuaternion::identity(),
975            )
976            .unwrap();
977
978        let pose_in_f2 = f2.add_pose(Vector3::new(1.0, 1.0, 0.0), UnitQuaternion::identity());
979
980        let pose_in_root = pose_in_f2.in_frame(&root).unwrap();
981        let pos = pose_in_root.transformation().translation.vector;
982
983        // Total offset should be: f2 (0,2,0) + pose (1,1,0) + f1 (1,0,0)
984        assert!((pos - Vector3::new(2.0, 3.0, 0.0)).norm() < 1e-6);
985    }
986
987    #[test]
988    fn test_calibrate_child() {
989        let root = Frame::new_origin("root");
990
991        let reference_pose = root.add_pose(
992            Vector3::new(1.0, 2.0, 3.0),
993            UnitQuaternion::from_euler_angles(0.0, 0.0, std::f64::consts::FRAC_PI_2),
994        );
995
996        // Calibrate a child where the reference pose should appear at (0,0,0) with identity orientation.
997        let calibrated_frame = root
998            .calibrate_child(
999                "calibrated",
1000                Vector3::zeros(),
1001                UnitQuaternion::identity(),
1002                &reference_pose,
1003            )
1004            .unwrap();
1005
1006        let pose_in_calibrated = reference_pose.in_frame(&calibrated_frame).unwrap();
1007        let transformation = pose_in_calibrated.transformation();
1008
1009        assert!((transformation.translation.vector - Vector3::zeros()).norm() < 1e-6);
1010        assert!((transformation.rotation.angle() - 0.0).abs() < 1e-6);
1011
1012        // Verify the child's transform matches the reference pose's original transform.
1013        let calibrated_transformation = calibrated_frame.transformation().unwrap();
1014        assert!(
1015            (calibrated_transformation.translation.vector - Vector3::new(1.0, 2.0, 3.0)).norm()
1016                < 1e-6
1017        );
1018        assert!(
1019            (calibrated_transformation.rotation.angle() - std::f64::consts::FRAC_PI_2).abs() < 1e-6
1020        );
1021    }
1022
1023    #[test]
1024    fn test_to_json_and_apply_config() {
1025        let root = Frame::new_origin("root");
1026        let _ = root
1027            .add_child(
1028                "child",
1029                Vector3::new(1.0, 2.0, 3.0),
1030                UnitQuaternion::from_euler_angles(0.1, 0.2, 0.3),
1031            )
1032            .unwrap();
1033
1034        let json = root.to_json().unwrap();
1035        // roughly verify JSON structure
1036        assert!(json.contains(r#""name": "root""#));
1037        assert!(json.contains(r#""name": "child""#));
1038
1039        // Create a default tree with different transforms
1040        let default_root = Frame::new_origin("root");
1041        default_root
1042            .add_child(
1043                "child",
1044                Vector3::new(0.0, 0.0, 0.0),
1045                UnitQuaternion::identity(),
1046            )
1047            .unwrap();
1048
1049        // Apply config
1050        default_root.apply_config(&json).unwrap();
1051
1052        // Verify child transform updated
1053        let updated_child = default_root
1054            .children()
1055            .into_iter()
1056            .find(|c| c.name() == "child")
1057            .unwrap();
1058        let iso = updated_child.transformation().unwrap();
1059        assert_eq!(iso.translation.vector, Vector3::new(1.0, 2.0, 3.0));
1060        let (r, p, y) = iso.rotation.euler_angles();
1061        assert!((r - 0.1).abs() < 1e-6);
1062        assert!((p - 0.2).abs() < 1e-6);
1063        assert!((y - 0.3).abs() < 1e-6);
1064
1065        // Test partial: If config has extra, ignore it
1066        let partial_json = r#"
1067        {
1068            "name": "root",
1069            "position": [0.0, 0.0, 0.0],
1070            "orientation": [0.0, 0.0, 0.0, 1.0],
1071            "children": [
1072                {
1073                    "name": "child",
1074                    "position": [4.0, 5.0, 6.0],
1075                    "orientation": [0.0, 0.0, 0.0, 1.0],
1076                    "children": []
1077                },
1078                {
1079                    "name": "extra",
1080                    "position": [0.0, 0.0, 0.0],
1081                    "orientation": [0.0, 0.0, 0.0, 1.0],
1082                    "children": []
1083                }
1084            ]
1085        }
1086        "#;
1087        default_root.apply_config(partial_json).unwrap();
1088        let updated_child = default_root
1089            .children()
1090            .into_iter()
1091            .find(|c| c.name() == "child")
1092            .unwrap();
1093        assert_eq!(
1094            updated_child.transformation().unwrap().translation.vector,
1095            Vector3::new(4.0, 5.0, 6.0)
1096        );
1097
1098        // Test mismatch
1099        let mismatch_json = r#"
1100        {
1101            "name": "wrong_root",
1102            "position": [0.0, 0.0, 0.0],
1103            "orientation": [0.0, 0.0, 0.0, 1.0],
1104            "children": []
1105        }
1106        "#;
1107        assert!(default_root.apply_config(mismatch_json).is_err());
1108    }
1109
1110    #[test]
1111    fn test_lazy_translation_frame() {
1112        use nalgebra::UnitQuaternion;
1113
1114        let root = Frame::new_origin("root");
1115        let child = root
1116            .add_child(
1117                "child",
1118                Vector3::new(0.0, 0.0, 0.0),
1119                UnitQuaternion::identity(),
1120            )
1121            .unwrap();
1122
1123        let result = &child + z(5.0);
1124        assert_relative_eq!(
1125            result.transformation().unwrap().translation.vector,
1126            Vector3::new(0.0, 0.0, 5.0),
1127            epsilon = 1e-10
1128        );
1129        assert_relative_eq!(
1130            child.transformation().unwrap().translation.vector,
1131            Vector3::new(0.0, 0.0, 0.0),
1132            epsilon = 1e-10
1133        );
1134
1135        let result = &result - y(3.0);
1136        assert_relative_eq!(
1137            result.transformation().unwrap().translation.vector,
1138            Vector3::new(0.0, -3.0, 5.0),
1139            epsilon = 1e-10
1140        );
1141
1142        let (roll, pitch, yaw) = result.transformation().unwrap().rotation.euler_angles();
1143        assert_relative_eq!(
1144            Vector3::new(roll, pitch, yaw),
1145            Vector3::new(0.0, 0.0, 0.0),
1146            epsilon = 1e-10
1147        );
1148    }
1149
1150    #[test]
1151    fn test_lazy_rotation_frame() {
1152        use nalgebra::UnitQuaternion;
1153        let root = Frame::new_origin("root");
1154        let child = root
1155            .add_child(
1156                "child",
1157                Vector3::new(0.0, 0.0, 0.0),
1158                UnitQuaternion::identity(),
1159            )
1160            .unwrap();
1161        let result = &child * rz(std::f64::consts::FRAC_PI_4);
1162
1163        let (roll, pitch, yaw) = result.transformation().unwrap().rotation.euler_angles();
1164        assert_relative_eq!(
1165            Vector3::new(roll, pitch, yaw),
1166            Vector3::new(0.0, 0.0, std::f64::consts::FRAC_PI_4),
1167            epsilon = 1e-10
1168        );
1169        assert_relative_eq!(
1170            result.transformation().unwrap().translation.vector,
1171            Vector3::new(0.0, 0.0, 0.0),
1172            epsilon = 1e-10
1173        );
1174        let (roll, pitch, yaw) = child.transformation().unwrap().rotation.euler_angles();
1175        assert_relative_eq!(
1176            Vector3::new(roll, pitch, yaw),
1177            Vector3::new(0.0, 0.0, 0.0),
1178            epsilon = 1e-10
1179        );
1180    }
1181
1182    #[test]
1183    fn test_lazy_translation_pose() {
1184        use nalgebra::UnitQuaternion;
1185
1186        let root = Frame::new_origin("root");
1187        let pose = root.add_pose(Vector3::new(0.0, 0.0, 0.0), UnitQuaternion::identity());
1188
1189        let result = &pose + z(5.0);
1190        assert_relative_eq!(
1191            result.transformation().translation.vector,
1192            Vector3::new(0.0, 0.0, 5.0),
1193            epsilon = 1e-10
1194        );
1195        assert_relative_eq!(
1196            pose.transformation().translation.vector,
1197            Vector3::new(0.0, 0.0, 0.0),
1198            epsilon = 1e-10
1199        );
1200
1201        let result = &result - y(3.0);
1202        assert_relative_eq!(
1203            result.transformation().translation.vector,
1204            Vector3::new(0.0, -3.0, 5.0),
1205            epsilon = 1e-10
1206        );
1207
1208        let (roll, pitch, yaw) = result.transformation().rotation.euler_angles();
1209        assert_relative_eq!(
1210            Vector3::new(roll, pitch, yaw),
1211            Vector3::new(0.0, 0.0, 0.0),
1212            epsilon = 1e-10
1213        );
1214    }
1215
1216    #[test]
1217    fn test_lazy_rotation_pose() {
1218        use nalgebra::UnitQuaternion;
1219        let root = Frame::new_origin("root");
1220        let pose = root.add_pose(Vector3::new(0.0, 0.0, 0.0), UnitQuaternion::identity());
1221        let result = &pose * rz(std::f64::consts::FRAC_PI_4);
1222
1223        let (roll, pitch, yaw) = result.transformation().rotation.euler_angles();
1224        assert_relative_eq!(
1225            Vector3::new(roll, pitch, yaw),
1226            Vector3::new(0.0, 0.0, std::f64::consts::FRAC_PI_4),
1227            epsilon = 1e-10
1228        );
1229        assert_relative_eq!(
1230            result.transformation().translation.vector,
1231            Vector3::new(0.0, 0.0, 0.0),
1232            epsilon = 1e-10
1233        );
1234        let (roll, pitch, yaw) = pose.transformation().rotation.euler_angles();
1235        assert_relative_eq!(
1236            Vector3::new(roll, pitch, yaw),
1237            Vector3::new(0.0, 0.0, 0.0),
1238            epsilon = 1e-10
1239        );
1240    }
1241}
cartesian_tree/frame.rs

cartesian_tree/
frame.rs
