Skip to content

fix link transform calculation #157

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
McModknower wants to merge 2 commits into
base: main
Choose a base branch
from
Open

fix link transform calculation #157

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1106933
fix link transform calculation
McModknower Oct 10, 2025
33942fa
run cargo fmt
McModknower Oct 10, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
32 changes: 31 additions & 1 deletion src/chain.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -488,7 +488,7 @@ impl<T: RealField + SubsetOf<f64>> Chain<T> {
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();
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -820,4 +822,32 @@ 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
);
}
}
}
Loading