From 1106933c4c251a1da64b8bc2f73df0f8ba1f9e8e Mon Sep 17 00:00:00 2001 From: McModknower Date: Fri, 10 Oct 2025 15:17:22 +0200 Subject: [PATCH 1/2] fix link transform calculation --- src/chain.rs | 31 ++++++++++++++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/src/chain.rs b/src/chain.rs index c1f58b5..04166fd 100644 --- a/src/chain.rs +++ b/src/chain.rs @@ -488,7 +488,7 @@ impl> Chain { pub fn update_link_transforms(&self) { self.update_transforms(); self.iter().for_each(|node| { - let parent_transform = node.parent_world_transform().expect("cache must exist"); + let parent_transform = node.world_transform().expect("cache must exist"); let mut node_mut = node.lock(); if let Some(ref mut link) = node_mut.link { let inertial_trans = parent_transform.clone() * link.inertial.origin(); @@ -681,6 +681,8 @@ where #[cfg(test)] mod tests { + use crate::link::LinkBuilder; + use super::*; #[cfg(target_family = "wasm")] use wasm_bindgen_test::wasm_bindgen_test as test; @@ -820,4 +822,31 @@ mod tests { assert!((positions[1] - 0.2f64).abs() < f64::EPSILON); assert!((positions[2] - 0.9f64).abs() < f64::EPSILON); } + + #[test] + fn test_update_link_transforms() { + let joint0 = NodeBuilder::new() + .name("j0") + .joint_type(JointType::Fixed) + .into_node(); + let joint1 = NodeBuilder::new() + .translation(na::Translation3::new(1.0, 0.0, 0.0)) + .name("j1") + .joint_type(JointType::Fixed) + .into_node(); + joint1.set_parent(&joint0); + let link1 = LinkBuilder::new() + .name("l1") + .finalize(); + joint1.set_link(Some(link1)); + + let chain = Chain::from_root(joint0); + + chain.update_link_transforms(); + + for link in chain.iter_links() { + println!("{:?}", link.inertial.world_transform()); + assert!((link.inertial.world_transform().unwrap().translation.x - 1.0f64).abs() < f64::EPSILON); + } + } } From 33942faeefd8e124ae143cdd24c8d51b12aff483 Mon Sep 17 00:00:00 2001 From: McModknower Date: Fri, 10 Oct 2025 15:30:18 +0200 Subject: [PATCH 2/2] run cargo fmt --- src/chain.rs | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/chain.rs b/src/chain.rs index 04166fd..55db54f 100644 --- a/src/chain.rs +++ b/src/chain.rs @@ -834,19 +834,20 @@ mod tests { .name("j1") .joint_type(JointType::Fixed) .into_node(); - joint1.set_parent(&joint0); - let link1 = LinkBuilder::new() - .name("l1") - .finalize(); - joint1.set_link(Some(link1)); + joint1.set_parent(&joint0); + let link1 = LinkBuilder::new().name("l1").finalize(); + joint1.set_link(Some(link1)); - let chain = Chain::from_root(joint0); + let chain = Chain::from_root(joint0); - chain.update_link_transforms(); + chain.update_link_transforms(); - for link in chain.iter_links() { - println!("{:?}", link.inertial.world_transform()); - assert!((link.inertial.world_transform().unwrap().translation.x - 1.0f64).abs() < f64::EPSILON); - } + for link in chain.iter_links() { + println!("{:?}", link.inertial.world_transform()); + assert!( + (link.inertial.world_transform().unwrap().translation.x - 1.0f64).abs() + < f64::EPSILON + ); + } } }