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}