k/
node.rs

1/*
2  Copyright 2017 Takashi Ogura
3
4  Licensed under the Apache License, Version 2.0 (the "License");
5  you may not use this file except in compliance with the License.
6  You may obtain a copy of the License at
7
8      http://www.apache.org/licenses/LICENSE-2.0
9
10  Unless required by applicable law or agreed to in writing, software
11  distributed under the License is distributed on an "AS IS" BASIS,
12  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  See the License for the specific language governing permissions and
14  limitations under the License.
15*/
16//! graph structure for kinematic chain
17use na::{Isometry3, RealField, Translation3, UnitQuaternion};
18use nalgebra as na;
19use simba::scalar::SubsetOf;
20use std::fmt::{self, Display};
21use std::ops::Deref;
22use std::sync::{Arc, Mutex, MutexGuard, Weak};
23
24use super::errors::*;
25use super::iterator::*;
26use super::joint::*;
27use super::link::*;
28
29type WeakNode<T> = Weak<Mutex<NodeImpl<T>>>;
30
31/// Node for joint tree struct
32#[derive(Debug)]
33pub struct NodeImpl<T>
34where
35    T: RealField,
36{
37    pub parent: Option<WeakNode<T>>,
38    pub children: Vec<Node<T>>,
39    pub joint: Joint<T>,
40    pub mimic_parent: Option<WeakNode<T>>,
41    pub mimic_children: Vec<Node<T>>,
42    pub mimic: Option<Mimic<T>>,
43    pub link: Option<Link<T>>,
44}
45
46/// Parts of `Chain`
47///
48/// It contains joint, joint (transform), and parent/children.
49#[derive(Debug)]
50pub struct Node<T: RealField>(pub(crate) Arc<Mutex<NodeImpl<T>>>);
51
52impl<T> Node<T>
53where
54    T: RealField + SubsetOf<f64>,
55{
56    pub(crate) fn from_arc(arc_mutex_node: Arc<Mutex<NodeImpl<T>>>) -> Self {
57        Node(arc_mutex_node)
58    }
59
60    pub fn new(joint: Joint<T>) -> Self {
61        Node::<T>(Arc::new(Mutex::new(NodeImpl {
62            parent: None,
63            children: Vec::new(),
64            joint,
65            mimic_parent: None,
66            mimic_children: Vec::new(),
67            mimic: None,
68            link: None,
69        })))
70    }
71
72    pub(crate) fn lock(&self) -> MutexGuard<'_, NodeImpl<T>> {
73        self.0.lock().unwrap()
74    }
75
76    pub fn joint(&self) -> JointRefGuard<'_, T> {
77        JointRefGuard { guard: self.lock() }
78    }
79
80    pub fn joint_position(&self) -> Option<T> {
81        self.lock().joint.joint_position()
82    }
83
84    pub fn parent(&self) -> Option<Node<T>> {
85        match self.lock().parent {
86            Some(ref weak) => weak.upgrade().map(Node::from_arc),
87            None => None,
88        }
89    }
90
91    pub fn children(&self) -> ChildrenRefGuard<'_, T> {
92        ChildrenRefGuard { guard: self.lock() }
93    }
94
95    /// iter from the end to root, it contains `nodes[id]` itself
96    #[inline]
97    pub fn iter_ancestors(&self) -> Ancestors<T> {
98        Ancestors::new(Some(self.clone()))
99    }
100    /// iter to the end, it contains `nodes[id]` itself
101    #[inline]
102    pub fn iter_descendants(&self) -> Descendants<T> {
103        Descendants::new(vec![self.clone()])
104    }
105
106    /// Set parent and child relations at same time
107    pub fn set_parent(&self, parent: &Node<T>) {
108        self.lock().parent = Some(Arc::downgrade(&parent.0));
109        parent.0.lock().unwrap().children.push(self.clone());
110    }
111
112    /// Remove parent and child relations at same time
113    pub fn remove_parent(&self, parent: &Node<T>) {
114        self.lock().parent = None;
115        parent.0.lock().unwrap().children.retain(|x| *x != *self);
116    }
117
118    /// # Examples
119    ///
120    /// ```
121    /// use k::*;
122    ///
123    /// let l0 = k::NodeBuilder::<f32>::new().into_node();
124    /// let l1 = k::NodeBuilder::new().into_node();
125    /// l1.set_parent(&l0);
126    /// assert!(l0.is_root());
127    /// assert!(!l1.is_root());
128    /// ```
129    pub fn is_root(&self) -> bool {
130        self.lock().parent.is_none()
131    }
132
133    /// # Examples
134    ///
135    /// ```
136    /// let l0 = k::NodeBuilder::<f64>::new().into_node();
137    /// let l1 = k::NodeBuilder::new().into_node();
138    /// l1.set_parent(&l0);
139    /// assert!(!l0.is_end());
140    /// assert!(l1.is_end());
141    /// ```
142    pub fn is_end(&self) -> bool {
143        self.0.lock().unwrap().children.is_empty()
144    }
145
146    /// Set the origin transform of the joint
147    #[inline]
148    pub fn set_origin(&self, trans: Isometry3<T>) {
149        self.lock().joint.set_origin(trans);
150    }
151
152    /// Get the origin transform of the joint
153    #[inline]
154    pub fn origin(&self) -> Isometry3<T> {
155        self.joint().origin().clone()
156    }
157
158    /// Set the position (angle) of the joint
159    ///
160    /// If position is out of limit, it returns Err.
161    ///
162    /// # Examples
163    ///
164    /// ```
165    /// use k::*;
166    /// let l0 = NodeBuilder::new()
167    ///     .joint_type(JointType::Linear{axis: Vector3::z_axis()})
168    ///     .limits(Some((0.0..=2.0).into()))
169    ///     .into_node();
170    /// assert!(l0.set_joint_position(1.0).is_ok());
171    /// assert!(l0.set_joint_position(-1.0).is_err());
172    /// ```
173    ///
174    /// Setting position for Fixed joint is error.
175    ///
176    /// ```
177    /// use k::*;
178    /// let l0 = NodeBuilder::new()
179    ///     .joint_type(JointType::Fixed)
180    ///     .into_node();
181    /// assert!(l0.set_joint_position(0.0).is_err());
182    /// ```
183    ///
184    /// `k::joint::Mimic` can be used to copy other joint's position.
185    ///
186    /// ```
187    /// use k::*;
188    /// let j0 = NodeBuilder::new()
189    ///     .joint_type(JointType::Linear{axis: Vector3::z_axis()})
190    ///     .limits(Some((0.0..=2.0).into()))
191    ///     .into_node();
192    /// let j1 = NodeBuilder::new()
193    ///     .joint_type(JointType::Linear{axis: Vector3::z_axis()})
194    ///     .limits(Some((0.0..=2.0).into()))
195    ///     .into_node();
196    /// j1.set_mimic_parent(&j0, k::joint::Mimic::new(1.5, 0.1));
197    /// assert_eq!(j0.joint_position().unwrap(), 0.0);
198    /// assert_eq!(j1.joint_position().unwrap(), 0.0);
199    /// assert!(j0.set_joint_position(1.0).is_ok());
200    /// assert_eq!(j0.joint_position().unwrap(), 1.0);
201    /// assert_eq!(j1.joint_position().unwrap(), 1.6);
202    /// ```
203    pub fn set_joint_position(&self, position: T) -> Result<(), Error> {
204        let mut node = self.lock();
205        if node.mimic_parent.is_some() {
206            return Ok(());
207        }
208        node.joint.set_joint_position(position.clone())?;
209        for child in &node.mimic_children {
210            let mut child_node = child.lock();
211            let mimic = child_node.mimic.clone();
212            match mimic {
213                Some(m) => child_node
214                    .joint
215                    .set_joint_position(m.mimic_position(position.clone()))?,
216                None => {
217                    let from = self.joint().name.to_owned();
218                    let to = child.joint().name.to_owned();
219                    return Err(Error::MimicError { from, to });
220                }
221            };
222        }
223        Ok(())
224    }
225
226    /// Set the clamped position (angle) of the joint
227    ///
228    /// It refers to the joint limit and clamps the argument. This function does nothing if this is fixed joint.
229    ///
230    /// # Examples
231    ///
232    /// ```
233    /// use k::*;
234    /// let l0 = NodeBuilder::new()
235    ///     .joint_type(JointType::Linear{axis: Vector3::z_axis()})
236    ///     .limits(Some((-1.0..=1.0).into()))
237    ///     .into_node();
238    /// l0.set_joint_position_clamped(2.0);
239    /// assert_eq!(l0.joint().joint_position(), Some(1.0));
240    /// l0.set_joint_position_clamped(-2.0);
241    /// assert_eq!(l0.joint().joint_position(), Some(-1.0));
242    /// ```
243    pub fn set_joint_position_clamped(&self, position: T) {
244        self.0
245            .lock()
246            .unwrap()
247            .joint
248            .set_joint_position_clamped(position);
249    }
250
251    #[inline]
252    pub fn set_joint_position_unchecked(&self, position: T) {
253        self.0
254            .lock()
255            .unwrap()
256            .joint
257            .set_joint_position_unchecked(position);
258    }
259
260    pub(crate) fn parent_world_transform(&self) -> Option<Isometry3<T>> {
261        //match self.0.borrow().parent {
262        match self.parent() {
263            Some(ref parent) => parent.world_transform(),
264            None => Some(Isometry3::identity()),
265        }
266    }
267
268    pub(crate) fn parent_world_velocity(&self) -> Option<Velocity<T>> {
269        match self.parent() {
270            Some(ref parent) => parent.world_velocity(),
271            None => Some(Velocity::zero()),
272        }
273    }
274
275    /// Get the calculated world transform.
276    /// Call `Chain::update_transforms()` before using this method.
277    ///
278    ///  # Examples
279    ///
280    /// ```
281    /// use k::*;
282    /// use k::prelude::*;
283    ///
284    /// let l0 = NodeBuilder::new()
285    ///     .translation(Translation3::new(0.0, 0.0, 0.2))
286    ///     .joint_type(JointType::Rotational{axis: Vector3::y_axis()})
287    ///     .into_node();
288    /// let l1 = NodeBuilder::new()
289    ///     .translation(Translation3::new(0.0, 0.0, 1.0))
290    ///     .joint_type(JointType::Linear{axis: Vector3::z_axis()})
291    ///     .into_node();
292    /// l1.set_parent(&l0);
293    /// let tree = Chain::<f64>::from_root(l0);
294    /// tree.set_joint_positions(&vec![3.141592 * 0.5, 0.1]).unwrap();
295    /// assert!(l1.world_transform().is_none());
296    /// assert!(l1.world_transform().is_none());
297    /// let _poses = tree.update_transforms();
298    /// assert!((l1.world_transform().unwrap().translation.vector.x - 1.1).abs() < 0.0001);
299    /// assert!((l1.world_transform().unwrap().translation.vector.z - 0.2).abs() < 0.0001);
300    ///
301    /// // _poses[0] is as same as l0.world_transform()
302    /// // _poses[1] is as same as l1.world_transform()
303    #[inline]
304    pub fn world_transform(&self) -> Option<Isometry3<T>> {
305        self.joint().world_transform()
306    }
307    #[inline]
308    pub fn world_velocity(&self) -> Option<Velocity<T>> {
309        self.joint().world_velocity()
310    }
311
312    pub fn mimic_parent(&self) -> Option<Node<T>> {
313        match self.lock().mimic_parent {
314            Some(ref weak) => weak.upgrade().map(Node::from_arc),
315            None => None,
316        }
317    }
318
319    pub fn set_mimic_parent(&self, parent: &Node<T>, mimic: Mimic<T>) {
320        self.lock().mimic_parent = Some(Arc::downgrade(&parent.0));
321        parent.lock().mimic_children.push(self.clone());
322        self.lock().mimic = Some(mimic);
323    }
324
325    pub fn set_link(&self, link: Option<Link<T>>) {
326        self.lock().link = link;
327    }
328
329    pub fn link(&self) -> OptionLinkRefGuard<'_, T> {
330        OptionLinkRefGuard { guard: self.lock() }
331    }
332}
333
334impl<T> ::std::clone::Clone for Node<T>
335where
336    T: RealField,
337{
338    fn clone(&self) -> Self {
339        Node::<T>(self.0.clone())
340    }
341}
342
343impl<T> PartialEq for Node<T>
344where
345    T: RealField,
346{
347    fn eq(&self, other: &Node<T>) -> bool {
348        std::ptr::eq(&*self.0, &*other.0)
349    }
350}
351
352impl<T: RealField + SubsetOf<f64>> Display for Node<T> {
353    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
354        let inner = self.lock();
355        inner.joint.fmt(f)?;
356
357        if let Some(l) = &inner.link {
358            write!(f, " => /{}/", l.name)?;
359        }
360        Ok(())
361    }
362}
363
364impl<T> From<Joint<T>> for Node<T>
365where
366    T: RealField + SubsetOf<f64>,
367{
368    fn from(joint: Joint<T>) -> Self {
369        Self::new(joint)
370    }
371}
372
373macro_rules! def_ref_guard {
374    ($guard_struct:ident, $target:ty, $member:ident) => {
375        #[derive(Debug)]
376        pub struct $guard_struct<'a, T>
377        where
378            T: RealField,
379        {
380            guard: MutexGuard<'a, NodeImpl<T>>,
381        }
382
383        impl<T> Deref for $guard_struct<'_, T>
384        where
385            T: RealField,
386        {
387            type Target = $target;
388            fn deref(&self) -> &Self::Target {
389                &self.guard.$member
390            }
391        }
392    };
393}
394
395/*
396
397macro_rules! def_ref_guard_mut {
398    ($guard_struct:ident, $target:ty, $member:ident) => {
399        pub struct $guard_struct<'a, T>
400        where
401            T: RealField,
402        {
403            guard: RefMut<'a, NodeImpl<T>>,
404        }
405
406        impl<T> Deref for $guard_struct<'_, T>
407        where
408            T: RealField,
409        {
410            type Target = $target;
411            fn deref(&self) -> &Self::Target {
412                &self.guard.$member
413            }
414        }
415
416        impl<T> DerefMut for $guard_struct<'_, T>
417        where
418            T: RealField,
419        {
420            fn deref_mut(&mut self) -> &mut $target {
421                &mut self.guard.$member
422            }
423        }
424    };
425}
426*/
427
428def_ref_guard!(JointRefGuard, Joint<T>, joint);
429def_ref_guard!(OptionLinkRefGuard, Option<Link<T>>, link);
430//def_ref_guard!(LinkRefGuard, Link<T>, link);
431def_ref_guard!(ChildrenRefGuard, Vec<Node<T>>, children);
432
433#[derive(Debug)]
434pub struct LinkRefGuard<'a, T>
435where
436    T: RealField,
437{
438    pub(crate) guard: MutexGuard<'a, NodeImpl<T>>,
439}
440
441impl<T> Deref for LinkRefGuard<'_, T>
442where
443    T: RealField,
444{
445    type Target = Link<T>;
446    fn deref(&self) -> &Self::Target {
447        // danger
448        self.guard.link.as_ref().unwrap()
449    }
450}
451
452//def_ref_guard!(ParentRefGuard, Option<WeakNode<T>>, parent);
453
454/*
455pub struct ParentRefGuard<'a, T>
456where
457    T: RealField,
458{
459    guard: Ref<'a, NodeImpl<T>>,
460    parent: Option<Node<T>>,
461}
462
463impl<'a, T>  ParentRefGuard<'a, T> where T:RealField {
464    pub fn new(guard: Ref<'a, NodeImpl<T>>) -> Self {
465        let parent = guard.parent.and_then(|weak| weak.upgrade().map(|arc| Node::from_arc(arc)));
466        Self {
467            guard,
468            parent,
469        }
470    }
471}
472*/
473/*
474impl<'a, T> Deref for ParentRefGuard<'a, T>
475where
476    T: RealField,
477{
478    type Target = Option<Node<T>>;
479    fn deref(&self) -> &Self::Target {
480        &self.parent
481    }
482}
483*/
484
485//def_ref_guard_mut!(JointRefGuardMut, Joint<T>, joint);
486
487/// Build a `Link<T>`
488///
489/// # Examples
490///
491/// ```
492/// use k::*;
493/// let l0 = NodeBuilder::new()
494///     .name("link_pitch")
495///     .translation(Translation3::new(0.0, 0.1, 0.0))
496///     .joint_type( JointType::Rotational{axis: Vector3::y_axis()})
497///     .finalize();
498/// println!("{l0:?}");
499/// ```
500#[derive(Debug, Clone)]
501pub struct NodeBuilder<T: RealField> {
502    name: String,
503    joint_type: JointType<T>,
504    limits: Option<Range<T>>,
505    origin: Isometry3<T>,
506}
507
508impl<T> Default for NodeBuilder<T>
509where
510    T: RealField + SubsetOf<f64>,
511{
512    fn default() -> Self {
513        Self::new()
514    }
515}
516
517impl<T> NodeBuilder<T>
518where
519    T: RealField + SubsetOf<f64>,
520{
521    pub fn new() -> NodeBuilder<T> {
522        NodeBuilder {
523            name: "".to_string(),
524            joint_type: JointType::Fixed,
525            limits: None,
526            origin: Isometry3::identity(),
527        }
528    }
529    /// Set the name of the `Link`
530    pub fn name(mut self, name: &str) -> NodeBuilder<T> {
531        self.name = name.to_string();
532        self
533    }
534    /// Set the joint which is connected to this link
535    pub fn joint_type(mut self, joint_type: JointType<T>) -> NodeBuilder<T> {
536        self.joint_type = joint_type;
537        self
538    }
539    /// Set joint limits
540    pub fn limits(mut self, limits: Option<Range<T>>) -> NodeBuilder<T> {
541        self.limits = limits;
542        self
543    }
544    /// Set the origin transform of this joint
545    pub fn origin(mut self, origin: Isometry3<T>) -> NodeBuilder<T> {
546        self.origin = origin;
547        self
548    }
549    /// Set the translation of the origin transform of this joint
550    pub fn translation(mut self, translation: Translation3<T>) -> NodeBuilder<T> {
551        self.origin.translation = translation;
552        self
553    }
554    /// Set the rotation of the origin transform of this joint
555    pub fn rotation(mut self, rotation: UnitQuaternion<T>) -> NodeBuilder<T> {
556        self.origin.rotation = rotation;
557        self
558    }
559    /// Create `Joint` instance
560    pub fn finalize(self) -> Joint<T> {
561        let mut joint = Joint::new(&self.name, self.joint_type);
562        joint.set_origin(self.origin);
563        joint.limits = self.limits;
564        joint
565    }
566    /// Create `Node` instead of `Joint` as output
567    pub fn into_node(self) -> Node<T> {
568        self.finalize().into()
569    }
570}
571
572/// set parents easily
573///
574/// ```
575/// use k::connect;
576/// # fn main() {
577/// let l0 = k::NodeBuilder::<f64>::new().into_node();
578/// let l1 = k::NodeBuilder::new().into_node();
579/// let l2 = k::NodeBuilder::new().into_node();
580///
581/// // This is the same as below
582/// // l1.set_parent(&l0);
583/// // l2.set_parent(&l1);
584/// connect![l0 => l1 => l2];
585///
586/// assert!(l0.is_root());
587/// assert!(!l1.is_root());
588/// assert!(!l1.is_end());
589/// assert!(l2.is_end());
590/// # }
591/// ```
592#[macro_export]
593macro_rules! connect {
594    ($x:expr => $y:expr) => {
595        $y.set_parent(&$x);
596    };
597    ($x:expr => $y:expr => $($rest:tt)+) => {
598        $y.set_parent(&$x);
599        $crate::connect!($y => $($rest)*);
600    };
601}