k/chain.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*/
16use super::errors::*;
17use super::joint::*;
18use super::node::*;
19use na::{Isometry3, RealField};
20use nalgebra as na;
21use simba::scalar::SubsetOf;
22use std::fmt::{self, Display};
23use std::ops::Deref;
24
25/// Kinematic Chain using `Node`
26///
27/// # Examples
28///
29/// ```
30/// use k::*;
31/// use k::prelude::*;
32///
33/// let l0 = NodeBuilder::new()
34/// .name("joint_pitch0")
35/// .translation(Translation3::new(0.0, 0.0, 0.1))
36/// .joint_type(JointType::Rotational{axis: Vector3::y_axis()})
37/// .into_node();
38/// let l1 = NodeBuilder::new()
39/// .name("joint_pitch1")
40/// .translation(Translation3::new(0.0, 0.0, 0.5))
41/// .joint_type(JointType::Rotational{axis: Vector3::y_axis()})
42/// .into_node();
43/// let l2 = NodeBuilder::new()
44/// .name("hand")
45/// .translation(Translation3::new(0.0, 0.0, 0.5))
46/// .joint_type(JointType::Fixed)
47/// .into_node();
48///
49/// // Sequential joints structure
50/// connect![l0 => l1 => l2];
51///
52/// let mut tree = Chain::from_root(l0);
53/// assert_eq!(tree.dof(), 2);
54///
55/// // Get joint positions
56/// let positions = tree.joint_positions();
57/// assert_eq!(positions.len(), 2);
58/// assert_eq!(positions[0], 0.0);
59/// assert_eq!(positions[1], 0.0);
60///
61/// // Get the initial joint transforms
62/// let transforms = tree.update_transforms();
63/// assert_eq!(transforms.len(), 3);
64/// assert_eq!(transforms[0].translation.vector.z, 0.1);
65/// assert_eq!(transforms[1].translation.vector.z, 0.6);
66/// assert_eq!(transforms[2].translation.vector.z, 1.1);
67/// for t in transforms {
68/// println!("before: {t}");
69/// }
70///
71/// // Set joint positions
72/// tree.set_joint_positions(&vec![1.0, 2.0]).unwrap();
73/// let positions = tree.joint_positions();
74/// assert_eq!(positions[0], 1.0);
75/// assert_eq!(positions[1], 2.0);
76///
77/// // Get the result of forward kinematics
78/// let transforms = tree.update_transforms();
79/// assert_eq!(transforms.len(), 3);
80/// for t in transforms {
81/// println!("after: {t}");
82/// }
83/// ```
84#[derive(Debug)]
85pub struct Chain<T: RealField> {
86 nodes: Vec<Node<T>>,
87 movable_nodes: Vec<Node<T>>,
88 dof: usize,
89}
90
91impl<T: RealField + SubsetOf<f64>> Chain<T> {
92 fn fmt_with_indent_level(
93 &self,
94 node: &Node<T>,
95 level: usize,
96 f: &mut fmt::Formatter<'_>,
97 ) -> fmt::Result {
98 if self.nodes.iter().any(|joint| joint == node) {
99 writeln!(f, "{}{node}", " ".repeat(level))?;
100 }
101 for c in node.children().iter() {
102 self.fmt_with_indent_level(c, level + 1, f)?
103 }
104 Ok(())
105 }
106}
107
108impl<T: RealField + SubsetOf<f64>> Display for Chain<T> {
109 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
110 self.fmt_with_indent_level(self.iter().next().unwrap(), 0, f)
111 }
112}
113
114impl<T: RealField + SubsetOf<f64>> Chain<T> {
115 /// Create Chain from root joint
116 ///
117 /// # Examples
118 ///
119 /// ```
120 /// use k;
121 ///
122 /// let l0 = k::NodeBuilder::new().into_node();
123 /// let l1 = k::NodeBuilder::new().into_node();
124 /// l1.set_parent(&l0);
125 /// let tree = k::Chain::<f32>::from_root(l0);
126 /// ```
127 #[allow(clippy::needless_pass_by_value)]
128 pub fn from_root(root_joint: Node<T>) -> Self {
129 let nodes = root_joint.iter_descendants().collect::<Vec<_>>();
130 Self::from_nodes(nodes)
131 }
132
133 /// Create `Chain` from end joint. It has any branches.
134 ///
135 /// Do not discard root joint before create Chain.
136 /// If you want to get Chain, `unwrap()` this, it is safe.
137 ///
138 /// # Examples
139 ///
140 ///
141 /// ```
142 /// use k::*;
143 ///
144 /// fn create_tree_from_end() -> Chain<f64> {
145 /// let l0 = Node::new(Joint::new("fixed0", JointType::Fixed));
146 /// let l1 = Node::new(Joint::new("fixed1", JointType::Fixed));
147 /// l1.set_parent(&l0);
148 /// Chain::from_end(&l1) // ok, because root is stored in `Chain`
149 /// }
150 ///
151 /// let tree = create_tree_from_end(); // no problem
152 /// ```
153 pub fn from_end(end_joint: &Node<T>) -> Chain<T> {
154 let mut nodes = end_joint.iter_ancestors().collect::<Vec<_>>();
155 nodes.reverse();
156 Self::from_nodes(nodes)
157 }
158
159 /// Create `Chain` from nodes.
160 ///
161 /// This method is public, but it is for professional use.
162 ///
163 /// # Examples
164 ///
165 ///
166 /// ```
167 /// use k::*;
168 ///
169 /// let l0 = Node::new(Joint::new("fixed0", JointType::Fixed));
170 /// let l1 = Node::new(Joint::new("fixed1", JointType::Fixed));
171 /// l1.set_parent(&l0);
172 /// let chain = Chain::<f64>::from_nodes(vec![l0, l1]);
173 /// ```
174 pub fn from_nodes(nodes: Vec<Node<T>>) -> Chain<T> {
175 let movable_nodes = nodes
176 .iter()
177 .filter(|joint| joint.joint().is_movable())
178 .cloned()
179 .collect::<Vec<_>>();
180 Chain {
181 dof: movable_nodes.len(),
182 movable_nodes,
183 nodes,
184 }
185 }
186
187 /// Create `Chain` from end node and root node, without any branches.
188 /// The root node is included in the chain.
189 ///
190 /// # Examples
191 ///
192 /// ```
193 /// use k::*;
194 ///
195 /// let l0 = Node::new(Joint::new("fixed0", JointType::Fixed));
196 /// let l1 = Node::new(Joint::new("fixed1", JointType::Fixed));
197 /// let l2 = Node::new(Joint::new("fixed2", JointType::Fixed));
198 /// let l3 = Node::new(Joint::new("fixed3", JointType::Fixed));
199 /// l1.set_parent(&l0);
200 /// l2.set_parent(&l1);
201 /// l3.set_parent(&l2);
202 /// let chain = Chain::<f32>::from_end_to_root(&l2, &l1);
203 ///
204 /// assert!(chain.find("fixed0").is_none()); // not included
205 /// assert!(chain.find("fixed1").is_some());
206 /// assert!(chain.find("fixed2").is_some());
207 /// assert!(chain.find("fixed3").is_none()); // not included
208 /// ```
209 pub fn from_end_to_root(end_joint: &Node<T>, root_joint: &Node<T>) -> Chain<T> {
210 let mut nodes = Vec::new();
211 for n in end_joint.iter_ancestors() {
212 nodes.push(n.clone());
213 if n == *root_joint {
214 break;
215 }
216 }
217 nodes.reverse();
218 Self::from_nodes(nodes)
219 }
220
221 /// Set the `Chain`'s origin
222 ///
223 /// # Examples
224 ///
225 ///
226 /// ```
227 /// use k::*;
228 ///
229 /// let l0 = Node::new(Joint::new("fixed0", JointType::Fixed));
230 /// let l1 = Node::new(Joint::new("fixed1", JointType::Fixed));
231 /// l1.set_parent(&l0);
232 /// let c = Chain::<f32>::from_end(&l1);
233 /// let mut o = c.origin();
234 /// assert!(o.translation.vector[0].abs() < 0.000001);
235 /// o.translation.vector[0] = 1.0;
236 /// c.set_origin(o);
237 /// assert!((o.translation.vector[0] - 1.0).abs() < 0.000001);
238 /// ```
239 pub fn set_origin(&self, pose: na::Isometry3<T>) {
240 self.nodes[0].set_origin(pose)
241 }
242
243 /// Get the `Chain`'s origin
244 pub fn origin(&self) -> na::Isometry3<T> {
245 self.nodes[0].origin()
246 }
247
248 /// Iterate for all joint nodes
249 ///
250 /// The order is from parent to children. You can assume that parent is already iterated.
251 ///
252 /// # Examples
253 ///
254 /// ```
255 /// use k::*;
256 ///
257 /// let l0 = Node::new(Joint::new("fixed0", JointType::Fixed));
258 /// let l1 = Node::new(Joint::new("fixed1", JointType::Fixed));
259 /// l1.set_parent(&l0);
260 /// let tree = Chain::<f64>::from_root(l0);
261 /// let names = tree.iter().map(|node| node.joint().name.to_owned()).collect::<Vec<_>>();
262 /// assert_eq!(names.len(), 2);
263 /// assert_eq!(names[0], "fixed0");
264 /// assert_eq!(names[1], "fixed1");
265 /// ```
266 pub fn iter(&self) -> impl Iterator<Item = &Node<T>> {
267 self.nodes.iter()
268 }
269
270 /// Iterate for movable joints
271 ///
272 /// Fixed joints are ignored. If you want to manipulate on Fixed,
273 /// use `iter()` instead of `iter_joints()`
274 pub fn iter_joints(&self) -> impl Iterator<Item = JointRefGuard<'_, T>> {
275 self.movable_nodes.iter().map(|node| node.joint())
276 }
277
278 /// Iterate for links
279 pub fn iter_links(&self) -> impl Iterator<Item = LinkRefGuard<'_, T>> {
280 self.nodes.iter().filter_map(|node| {
281 if node.link().is_some() {
282 Some(LinkRefGuard { guard: node.lock() })
283 } else {
284 None
285 }
286 })
287 }
288 /// Calculate the degree of freedom
289 ///
290 /// # Examples
291 ///
292 /// ```
293 /// use k::*;
294 /// let l0 = NodeBuilder::new()
295 /// .joint_type(JointType::Fixed)
296 /// .finalize()
297 /// .into();
298 /// let l1 : Node<f64> = NodeBuilder::new()
299 /// .joint_type(JointType::Rotational{axis: Vector3::y_axis()})
300 /// .finalize()
301 /// .into();
302 /// l1.set_parent(&l0);
303 /// let tree = Chain::from_root(l0);
304 /// assert_eq!(tree.dof(), 1);
305 /// ```
306 pub fn dof(&self) -> usize {
307 self.dof
308 }
309 /// Find the joint by name
310 ///
311 /// # Examples
312 ///
313 /// ```
314 /// use k::*;
315 ///
316 /// let l0 = Node::new(NodeBuilder::new()
317 /// .name("fixed")
318 /// .finalize());
319 /// let l1 = Node::new(NodeBuilder::new()
320 /// .name("pitch1")
321 /// .translation(Translation3::new(0.0, 0.1, 0.0))
322 /// .joint_type(JointType::Rotational{axis: Vector3::y_axis()})
323 /// .finalize());
324 /// l1.set_parent(&l0);
325 /// let tree = Chain::from_root(l0);
326 /// let j = tree.find("pitch1").unwrap();
327 /// j.set_joint_position(0.5).unwrap();
328 /// assert_eq!(j.joint_position().unwrap(), 0.5);
329 /// ```
330 pub fn find(&self, joint_name: &str) -> Option<&Node<T>> {
331 self.iter().find(|joint| joint.joint().name == joint_name)
332 }
333 /// Find the joint by link name
334 ///
335 /// # Examples
336 ///
337 /// ```
338 /// use k::*;
339 ///
340 /// let l0 = Node::new(NodeBuilder::new()
341 /// .name("fixed")
342 /// .finalize());
343 /// l0.set_link(Some(link::LinkBuilder::new().name("link0").finalize()));
344 /// let mut l1 = Node::new(NodeBuilder::new()
345 /// .name("pitch1")
346 /// .translation(Translation3::new(0.0, 0.1, 0.0))
347 /// .joint_type(JointType::Rotational{axis: Vector3::y_axis()})
348 /// .finalize());
349 /// l1.set_parent(&l0);
350 /// l1.set_link(Some(link::LinkBuilder::new().name("link1").finalize()));
351 /// let tree = Chain::from_root(l0);
352 /// let joint_name = tree.find_link("link1").unwrap()
353 /// .joint().name.clone();
354 /// assert_eq!(joint_name, "pitch1");
355 /// ```
356 pub fn find_link(&self, link_name: &str) -> Option<&Node<T>> {
357 self.iter().find(|node| match node.link().as_ref() {
358 Some(link) => link.name == link_name,
359 None => false,
360 })
361 }
362
363 /// Get the positions of the joints
364 ///
365 /// `FixedJoint` is ignored. the length is the same with `dof()`
366 pub fn joint_positions(&self) -> Vec<T> {
367 self.iter_joints()
368 .map(|joint| {
369 joint
370 .joint_position()
371 .expect("Must be a bug: movable joint must have position")
372 })
373 .collect()
374 }
375
376 /// Set the positions of the joints
377 ///
378 /// `FixedJoints` are ignored. the input number must be equal with `dof()`
379 pub fn set_joint_positions(&self, positions_vec: &[T]) -> Result<(), Error> {
380 if positions_vec.len() != self.dof {
381 return Err(Error::SizeMismatchError {
382 input: positions_vec.len(),
383 required: self.dof,
384 });
385 }
386 for (joint, position) in self.movable_nodes.iter().zip(positions_vec.iter()) {
387 joint.set_joint_position(position.clone())?;
388 }
389 Ok(())
390 }
391
392 /// Set the clamped positions of the joints
393 ///
394 /// This function is safe, in contrast to `set_joint_positions_unchecked`.
395 pub fn set_joint_positions_clamped(&self, positions_vec: &[T]) {
396 for (joint, position) in self.movable_nodes.iter().zip(positions_vec.iter()) {
397 joint.set_joint_position_clamped(position.clone());
398 }
399 }
400
401 /// Fast, but without check, dangerous `set_joint_positions`
402 #[inline]
403 pub fn set_joint_positions_unchecked(&self, positions_vec: &[T]) {
404 for (joint, position) in self.movable_nodes.iter().zip(positions_vec.iter()) {
405 joint.set_joint_position_unchecked(position.clone());
406 }
407 }
408
409 /// Update world_transform() of the joints
410 pub fn update_transforms(&self) -> Vec<Isometry3<T>> {
411 self.iter()
412 .map(|node| {
413 // copy the cache once to resolve a deadlock of 'node' guards
414 let cached_world_transform = node.joint().world_transform();
415 cached_world_transform.unwrap_or_else(|| {
416 // clear child caches
417 // recursive clearing is not necessary thanks to the order of Chain::iter()
418 for child in node.children().iter() {
419 child.joint().clear_caches();
420 }
421
422 // calculate the transformation
423 let parent_transform = node.parent_world_transform().expect("cache must exist");
424 let trans = parent_transform * node.joint().local_transform();
425 node.joint().set_world_transform(trans.clone());
426 trans
427 })
428 })
429 .collect()
430 }
431
432 /// Update world_velocity() of the joints
433 pub fn update_velocities(&self) -> Vec<Velocity<T>> {
434 self.update_transforms();
435 self.iter()
436 .map(|node| {
437 // copy the cache to resolve a deadlock of 'node' guards
438 let cached_world_velocity = node.joint().world_velocity();
439 cached_world_velocity.unwrap_or_else(|| {
440 let parent_transform = node
441 .parent_world_transform()
442 .expect("transform cache must exist");
443 let parent_velocity = node
444 .parent_world_velocity()
445 .expect("velocity cache must exist");
446 let velocity = match &node.joint().joint_type {
447 JointType::Fixed => parent_velocity,
448 JointType::Rotational { axis } => {
449 let parent = node.parent().expect("parent must exist");
450 let parent_vel = parent.joint().origin().translation.vector.clone();
451 Velocity::from_parts(
452 parent_velocity.translation
453 + parent_velocity.rotation.cross(
454 &(parent_transform.rotation.to_rotation_matrix()
455 * parent_vel),
456 ),
457 parent_velocity.rotation
458 + node
459 .world_transform()
460 .expect("cache must exist")
461 .rotation
462 .to_rotation_matrix()
463 * (axis.clone().into_inner()
464 * node.joint().joint_velocity().unwrap()),
465 )
466 }
467 JointType::Linear { axis } => Velocity::from_parts(
468 parent_velocity.translation
469 + node
470 .world_transform()
471 .expect("cache must exist")
472 .rotation
473 .to_rotation_matrix()
474 * (axis.clone().into_inner()
475 * node.joint().joint_velocity().unwrap()),
476 // TODO: Is this true??
477 parent_velocity.rotation,
478 ),
479 };
480 node.joint().set_world_velocity(velocity.clone());
481 velocity
482 })
483 })
484 .collect()
485 }
486
487 /// Update transforms of the links
488 pub fn update_link_transforms(&self) {
489 self.update_transforms();
490 self.iter().for_each(|node| {
491 let parent_transform = node.parent_world_transform().expect("cache must exist");
492 let mut node_mut = node.lock();
493 if let Some(ref mut link) = node_mut.link {
494 let inertial_trans = parent_transform.clone() * link.inertial.origin();
495 link.inertial.set_world_transform(inertial_trans);
496 for c in &mut link.collisions {
497 let c_trans = parent_transform.clone() * c.origin();
498 c.set_world_transform(c_trans);
499 }
500 for v in &mut link.visuals {
501 let v_trans = parent_transform.clone() * v.origin();
502 v.set_world_transform(v_trans);
503 }
504 }
505 });
506 }
507}
508
509impl<T> Clone for Chain<T>
510where
511 T: RealField + SubsetOf<f64>,
512{
513 fn clone(&self) -> Self {
514 // first node must be root
515 if self.nodes.is_empty() {
516 return Self {
517 nodes: vec![],
518 movable_nodes: vec![],
519 dof: 0,
520 };
521 }
522 assert!(self.nodes[0].is_root());
523 // Clone everything
524 let mut new_nodes = self
525 .nodes
526 .iter()
527 .map(|n| {
528 let node = Node::new(n.joint().clone());
529 node.set_link(n.link().clone());
530 node
531 })
532 .collect::<Vec<_>>();
533 // Connect to new nodes
534 for i in 0..new_nodes.len() {
535 if let Some(p) = self.nodes[i].parent() {
536 // get index of p
537 let parent_index = self.nodes.iter().position(|x| *x == p).unwrap();
538 new_nodes[i].set_parent(&new_nodes[parent_index]);
539 }
540 if let Some(m) = self.nodes[i].mimic_parent() {
541 let parent_index = self.nodes.iter().position(|x| *x == m).unwrap();
542 new_nodes[i].set_mimic_parent(
543 &new_nodes[parent_index],
544 self.nodes[i].lock().mimic.clone().unwrap(),
545 );
546 }
547 }
548 //
549 // first node must be root
550 assert!(new_nodes[0].is_root());
551 Chain::from_root(new_nodes.remove(0))
552 }
553}
554
555#[derive(Debug)]
556/// Kinematic chain without any branch.
557///
558/// All joints are connected sequentially.
559pub struct SerialChain<T: RealField> {
560 inner: Chain<T>,
561}
562
563impl<T> SerialChain<T>
564where
565 T: RealField + SubsetOf<f64>,
566{
567 /// Convert Chain into SerialChain without any check
568 ///
569 /// If the input Chain has any branches it causes serious bugs.
570 ///
571 pub fn new_unchecked(inner: Chain<T>) -> Self {
572 Self { inner }
573 }
574 /// Convert Chain into SerialChain
575 ///
576 /// If the input Chain has any branches it fails.
577 ///
578 /// # Examples
579 ///
580 /// ```
581 /// let node = k::NodeBuilder::<f32>::new().into_node();
582 /// let chain = k::Chain::from_root(node);
583 /// assert!(k::SerialChain::try_new(chain).is_some());
584 /// ```
585 ///
586 /// ```
587 /// let node0 = k::NodeBuilder::<f32>::new().into_node();
588 /// let node1 = k::NodeBuilder::new().into_node();
589 /// let node2 = k::NodeBuilder::new().into_node();
590 /// node1.set_parent(&node0);
591 /// node2.set_parent(&node0);
592 /// let chain = k::Chain::from_root(node0);
593 /// assert!(k::SerialChain::try_new(chain).is_none());
594 /// ```
595 pub fn try_new(inner: Chain<T>) -> Option<Self> {
596 {
597 let num = inner.iter().count();
598 for node in inner.iter().take(num - 1) {
599 if node.children().len() != 1 {
600 return None;
601 }
602 }
603 }
604 Some(Self { inner })
605 }
606 /// Create SerialChain from the end `Node`
607 ///
608 /// # Examples
609 ///
610 /// ```
611 /// let node = k::NodeBuilder::<f32>::new().into_node();
612 /// let s_chain = k::SerialChain::from_end(&node);
613 /// ```
614 pub fn from_end(end_joint: &Node<T>) -> SerialChain<T> {
615 SerialChain {
616 inner: Chain::from_end(end_joint),
617 }
618 }
619
620 /// Create SerialChain from the end `Node` and root `Node`.
621 ///
622 /// # Examples
623 ///
624 /// ```
625 /// let node0 = k::NodeBuilder::<f32>::new().into_node();
626 /// let node1 = k::NodeBuilder::<f32>::new().into_node();
627 /// let node2 = k::NodeBuilder::<f32>::new().into_node();
628 /// let node3 = k::NodeBuilder::<f32>::new().into_node();
629 /// use k::connect;
630 /// connect![node0 => node1 => node2 => node3];
631 /// let s_chain = k::SerialChain::from_end_to_root(&node2, &node1);
632 /// assert_eq!(s_chain.iter().count(), 2);
633 /// ```
634 pub fn from_end_to_root(end_joint: &Node<T>, root_joint: &Node<T>) -> SerialChain<T> {
635 SerialChain {
636 inner: Chain::from_end_to_root(end_joint, root_joint),
637 }
638 }
639
640 /// Safely unwrap and returns inner `Chain` instance
641 pub fn unwrap(self) -> Chain<T> {
642 self.inner
643 }
644 /// Calculate transform of the end joint
645 pub fn end_transform(&self) -> Isometry3<T> {
646 self.iter().fold(Isometry3::identity(), |trans, joint| {
647 trans * joint.joint().local_transform()
648 })
649 }
650}
651
652impl<T> Clone for SerialChain<T>
653where
654 T: RealField + SubsetOf<f64>,
655{
656 fn clone(&self) -> Self {
657 Self {
658 inner: self.inner.clone(),
659 }
660 }
661}
662
663impl<T> Display for SerialChain<T>
664where
665 T: RealField + SubsetOf<f64>,
666{
667 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
668 self.inner.fmt(f)
669 }
670}
671
672impl<T> Deref for SerialChain<T>
673where
674 T: RealField,
675{
676 type Target = Chain<T>;
677 fn deref(&self) -> &Self::Target {
678 &self.inner
679 }
680}
681
682#[cfg(test)]
683mod tests {
684 use super::*;
685 #[cfg(target_family = "wasm")]
686 use wasm_bindgen_test::wasm_bindgen_test as test;
687
688 #[test]
689 fn test_chain0() {
690 let joint0 = NodeBuilder::new()
691 .name("j0")
692 .translation(na::Translation3::new(0.0, 0.1, 0.0))
693 .joint_type(JointType::Rotational {
694 axis: na::Vector3::y_axis(),
695 })
696 .into_node();
697 let joint1 = NodeBuilder::new()
698 .translation(na::Translation3::new(0.0, 0.1, 0.1))
699 .name("j1")
700 .joint_type(JointType::Rotational {
701 axis: na::Vector3::y_axis(),
702 })
703 .into_node();
704 let joint2 = NodeBuilder::new()
705 .name("j2")
706 .translation(na::Translation3::new(0.0, 0.1, 0.1))
707 .joint_type(JointType::Rotational {
708 axis: na::Vector3::y_axis(),
709 })
710 .into_node();
711 let joint3 = NodeBuilder::new()
712 .name("j3")
713 .translation(na::Translation3::new(0.0, 0.1, 0.2))
714 .joint_type(JointType::Rotational {
715 axis: na::Vector3::y_axis(),
716 })
717 .into_node();
718 let joint4 = NodeBuilder::new()
719 .name("j4")
720 .translation(na::Translation3::new(0.0, 0.1, 0.1))
721 .joint_type(JointType::Rotational {
722 axis: na::Vector3::y_axis(),
723 })
724 .into_node();
725 let joint5 = NodeBuilder::new()
726 .name("j5")
727 .translation(na::Translation3::new(0.0, 0.1, 0.1))
728 .joint_type(JointType::Rotational {
729 axis: na::Vector3::y_axis(),
730 })
731 .into_node();
732 joint1.set_parent(&joint0);
733 joint2.set_parent(&joint1);
734 joint3.set_parent(&joint2);
735 joint4.set_parent(&joint0);
736 joint5.set_parent(&joint4);
737
738 let names = joint0
739 .iter_descendants()
740 .map(|joint| joint.joint().name.clone())
741 .collect::<Vec<_>>();
742 println!("{joint0}");
743 assert_eq!(names.len(), 6);
744 println!("names = {names:?}");
745 let positions = joint0
746 .iter_descendants()
747 .map(|node| node.joint().joint_position())
748 .collect::<Vec<_>>();
749 println!("positions = {positions:?}");
750
751 fn get_z(joint: &Node<f32>) -> f32 {
752 match joint.parent_world_transform() {
753 Some(iso) => iso.translation.vector.z,
754 None => 0.0f32,
755 }
756 }
757
758 let poses = joint0
759 .iter_descendants()
760 .map(|joint| get_z(&joint))
761 .collect::<Vec<_>>();
762 println!("poses = {poses:?}");
763
764 let _ = joint0
765 .iter_ancestors()
766 .map(|joint| joint.set_joint_position(-0.5))
767 .collect::<Vec<_>>();
768 let positions = joint0
769 .iter_descendants()
770 .map(|node| node.joint().joint_position())
771 .collect::<Vec<_>>();
772 println!("positions = {positions:?}");
773
774 let poses = joint0
775 .iter_descendants()
776 .map(|joint| get_z(&joint))
777 .collect::<Vec<_>>();
778 println!("poses = {poses:?}");
779
780 let arm = Chain::from_end(&joint3);
781 assert_eq!(arm.joint_positions().len(), 4);
782 println!("{:?}", arm.joint_positions());
783 }
784
785 #[test]
786 fn test_mimic() {
787 let joint0 = NodeBuilder::new()
788 .name("j0")
789 .translation(na::Translation3::new(0.0, 0.1, 0.0))
790 .joint_type(JointType::Rotational {
791 axis: na::Vector3::y_axis(),
792 })
793 .into_node();
794 let joint1 = NodeBuilder::new()
795 .name("joint1")
796 .translation(na::Translation3::new(0.0, 0.1, 0.1))
797 .joint_type(JointType::Rotational {
798 axis: na::Vector3::y_axis(),
799 })
800 .into_node();
801 let joint2 = NodeBuilder::new()
802 .name("joint2")
803 .translation(na::Translation3::new(0.0, 0.1, 0.1))
804 .joint_type(JointType::Rotational {
805 axis: na::Vector3::y_axis(),
806 })
807 .into_node();
808 joint1.set_parent(&joint0);
809 joint2.set_parent(&joint1);
810 joint2.set_mimic_parent(&joint1, Mimic::new(2.0, 0.5));
811
812 let arm = Chain::from_root(joint0);
813
814 assert_eq!(arm.joint_positions().len(), 3);
815 println!("{:?}", arm.joint_positions());
816 let positions = vec![0.1, 0.2, 0.3];
817 arm.set_joint_positions(&positions).unwrap();
818 let positions = arm.joint_positions();
819 assert!((positions[0] - 0.1f64).abs() < f64::EPSILON);
820 assert!((positions[1] - 0.2f64).abs() < f64::EPSILON);
821 assert!((positions[2] - 0.9f64).abs() < f64::EPSILON);
822 }
823}