From 8fc465397b49f9d74a931a794a45dcad1a46532b Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Thu, 27 Nov 2025 11:07:44 +0100 Subject: [PATCH 1/9] wip --- .../store/re_data_loader/src/loader_urdf.rs | 128 ++++++++++++++++-- 1 file changed, 115 insertions(+), 13 deletions(-) diff --git a/crates/store/re_data_loader/src/loader_urdf.rs b/crates/store/re_data_loader/src/loader_urdf.rs index 5ed5fcd498d5..a8bf350bdb33 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -6,13 +6,13 @@ use std::{ use ahash::{HashMap, HashMapExt as _, HashSet, HashSetExt as _}; use anyhow::{Context as _, bail}; use itertools::Itertools as _; -use urdf_rs::{Geometry, Joint, Link, Material, Robot, Vec3, Vec4}; +use urdf_rs::{Geometry, Joint, Link, LinkName, Material, Robot, Vec3, Vec4}; use re_chunk::{ChunkBuilder, ChunkId, EntityPath, RowId, TimePoint}; use re_log_types::{EntityPathPart, StoreId}; use re_types::{ AsComponents, Component as _, ComponentDescriptor, SerializedComponentBatch, - archetypes::{Asset3D, Transform3D}, + archetypes::{Asset3D, CoordinateFrame, Transform3D, TransformAxes3D}, datatypes::Vec3D, external::glam, }; @@ -282,6 +282,15 @@ fn log_robot( .map(|prefix| prefix / EntityPath::from_single_string(urdf_tree.name.clone())) .unwrap_or_else(|| EntityPath::from_single_string(urdf_tree.name.clone())); + // Log the robot's root coordinate frame_id. + send_archetype( + tx, + store_id, + entity_path.clone(), + timepoint, + &CoordinateFrame::update_fields().with_frame_id(urdf_tree.root.name.clone()), + )?; + walk_tree( &urdf_tree, tx, @@ -345,8 +354,8 @@ fn log_joint( name: _, joint_type, origin, - parent: _, - child: _, + parent, + child, axis, limit, calibration, @@ -355,7 +364,24 @@ fn log_joint( safety_controller, } = joint; - send_transform(tx, store_id, joint_path.clone(), origin, timepoint)?; + // A joint's own coordinate frame is that of its parent link. + send_archetype( + tx, + store_id, + joint_path.clone(), + timepoint, + &CoordinateFrame::update_fields().with_frame_id(parent.link.clone()), + )?; + // Send the joint origin, i.e. the default transform from parent link to child link. + send_transform( + tx, + store_id, + joint_path.clone(), + origin, + timepoint, + Some(parent), + Some(child), + )?; log_debug_format( tx, @@ -404,13 +430,23 @@ fn log_joint( Ok(()) } -fn transform_from_pose(origin: &urdf_rs::Pose) -> Transform3D { +fn transform_from_pose( + origin: &urdf_rs::Pose, + parent: Option<&LinkName>, + child: Option<&LinkName>, +) -> Transform3D { let urdf_rs::Pose { xyz, rpy } = origin; let translation = [xyz[0] as f32, xyz[1] as f32, xyz[2] as f32]; let quaternion = quat_xyzw_from_roll_pitch_yaw(rpy[0] as f32, rpy[1] as f32, rpy[2] as f32); - Transform3D::update_fields() + let transform = Transform3D::update_fields() .with_translation(translation) - .with_quaternion(quaternion) + .with_quaternion(quaternion); + if let (Some(parent), Some(child)) = (parent, child) { + return transform + .with_parent_frame(parent.link.clone()) + .with_child_frame(child.link.clone()); + } + transform } fn send_transform( @@ -419,6 +455,8 @@ fn send_transform( entity_path: EntityPath, origin: &urdf_rs::Pose, timepoint: &TimePoint, + parent: Option<&LinkName>, + child: Option<&LinkName>, ) -> anyhow::Result<()> { let urdf_rs::Pose { xyz, rpy } = origin; let is_identity = xyz.0 == [0.0, 0.0, 0.0] && rpy.0 == [0.0, 0.0, 0.0]; @@ -426,12 +464,20 @@ fn send_transform( if is_identity { Ok(()) // avoid noise } else { + // TODO: remove axis log this after debugging + send_archetype( + tx, + store_id, + entity_path.clone(), + timepoint, + &TransformAxes3D::update_fields().with_axis_length(0.1), + )?; send_archetype( tx, store_id, entity_path, timepoint, - &transform_from_pose(origin), + &transform_from_pose(origin, parent, child), ) } } @@ -485,6 +531,19 @@ fn log_link( timepoint, )?; + // Log coordinate frame ID of the link. + send_archetype( + tx, + store_id, + link_entity.clone(), + timepoint, + &CoordinateFrame::update_fields().with_frame_id(link.name.clone()), + )?; + + let link_parent = urdf_tree + .get_parent_of_link(&link.name) + .map(|joint| &joint.child); + for (i, visual) in visual.iter().enumerate() { let urdf_rs::Visual { name, @@ -493,7 +552,7 @@ fn log_link( material, } = visual; let name = name.clone().unwrap_or_else(|| format!("visual_{i}")); - let vis_entity = link_entity / EntityPathPart::new(name); + let vis_entity = link_entity / EntityPathPart::new(name.clone()); // Prefer inline defined material properties if present, otherwise fall back to global material. let material = material.as_ref().and_then(|mat| { @@ -504,7 +563,29 @@ fn log_link( } }); - send_transform(tx, store_id, vis_entity.clone(), origin, timepoint)?; + let link_child = urdf_rs::LinkName { link: name }; + // TODO + send_transform( + tx, + store_id, + vis_entity.clone(), + origin, + timepoint, + link_parent, + Some(&link_child), + )?; + + if let Some(parent) = link_parent { + let coordinate_frame = + CoordinateFrame::update_fields().with_frame_id(parent.link.clone()); + send_archetype( + tx, + store_id, + vis_entity.clone(), + timepoint, + &coordinate_frame, + )?; + } log_geometry( urdf_tree, tx, store_id, vis_entity, geometry, material, timepoint, @@ -518,9 +599,30 @@ fn log_link( geometry, } = collision; let name = name.clone().unwrap_or_else(|| format!("collision_{i}")); - let collision_entity = link_entity / EntityPathPart::new(name); + let collision_entity = link_entity / EntityPathPart::new(name.clone()); - send_transform(tx, store_id, collision_entity.clone(), origin, timepoint)?; + let link_child = urdf_rs::LinkName { link: name }; + send_transform( + tx, + store_id, + collision_entity.clone(), + origin, + timepoint, + link_parent, + Some(&link_child), + )?; + + if let Some(parent) = link_parent { + let coordinate_frame = + CoordinateFrame::update_fields().with_frame_id(parent.link.clone()); + send_archetype( + tx, + store_id, + collision_entity.clone(), + timepoint, + &coordinate_frame, + )?; + } log_geometry( urdf_tree, From cc34b21e8ca6a4918830e70e898ef37f18a84524 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Thu, 27 Nov 2025 12:55:38 +0100 Subject: [PATCH 2/9] Clean up a bit --- .../store/re_data_loader/src/loader_urdf.rs | 136 ++++++++---------- 1 file changed, 59 insertions(+), 77 deletions(-) diff --git a/crates/store/re_data_loader/src/loader_urdf.rs b/crates/store/re_data_loader/src/loader_urdf.rs index a8bf350bdb33..9e411f00ed41 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -6,7 +6,7 @@ use std::{ use ahash::{HashMap, HashMapExt as _, HashSet, HashSetExt as _}; use anyhow::{Context as _, bail}; use itertools::Itertools as _; -use urdf_rs::{Geometry, Joint, Link, LinkName, Material, Robot, Vec3, Vec4}; +use urdf_rs::{Geometry, Joint, Link, Material, Robot, Vec3, Vec4}; use re_chunk::{ChunkBuilder, ChunkId, EntityPath, RowId, TimePoint}; use re_log_types::{EntityPathPart, StoreId}; @@ -282,7 +282,7 @@ fn log_robot( .map(|prefix| prefix / EntityPath::from_single_string(urdf_tree.name.clone())) .unwrap_or_else(|| EntityPath::from_single_string(urdf_tree.name.clone())); - // Log the robot's root coordinate frame_id. + // The robot's root coordinate frame_id. send_archetype( tx, store_id, @@ -379,8 +379,8 @@ fn log_joint( joint_path.clone(), origin, timepoint, - Some(parent), - Some(child), + parent.link.clone(), + child.link.clone(), )?; log_debug_format( @@ -432,21 +432,17 @@ fn log_joint( fn transform_from_pose( origin: &urdf_rs::Pose, - parent: Option<&LinkName>, - child: Option<&LinkName>, + parent_frame: String, + child_frame: String, ) -> Transform3D { let urdf_rs::Pose { xyz, rpy } = origin; let translation = [xyz[0] as f32, xyz[1] as f32, xyz[2] as f32]; let quaternion = quat_xyzw_from_roll_pitch_yaw(rpy[0] as f32, rpy[1] as f32, rpy[2] as f32); - let transform = Transform3D::update_fields() + Transform3D::update_fields() .with_translation(translation) - .with_quaternion(quaternion); - if let (Some(parent), Some(child)) = (parent, child) { - return transform - .with_parent_frame(parent.link.clone()) - .with_child_frame(child.link.clone()); - } - transform + .with_quaternion(quaternion) + .with_parent_frame(parent_frame) + .with_child_frame(child_frame) } fn send_transform( @@ -455,31 +451,24 @@ fn send_transform( entity_path: EntityPath, origin: &urdf_rs::Pose, timepoint: &TimePoint, - parent: Option<&LinkName>, - child: Option<&LinkName>, + parent_frame: String, + child_frame: String, ) -> anyhow::Result<()> { - let urdf_rs::Pose { xyz, rpy } = origin; - let is_identity = xyz.0 == [0.0, 0.0, 0.0] && rpy.0 == [0.0, 0.0, 0.0]; - - if is_identity { - Ok(()) // avoid noise - } else { - // TODO: remove axis log this after debugging - send_archetype( - tx, - store_id, - entity_path.clone(), - timepoint, - &TransformAxes3D::update_fields().with_axis_length(0.1), - )?; - send_archetype( - tx, - store_id, - entity_path, - timepoint, - &transform_from_pose(origin, parent, child), - ) - } + // TODO: remove axis log this after debugging + send_archetype( + tx, + store_id, + entity_path.clone(), + timepoint, + &TransformAxes3D::update_fields().with_axis_length(0.1), + )?; + send_archetype( + tx, + store_id, + entity_path, + timepoint, + &transform_from_pose(origin, parent_frame, child_frame), + ) } /// Log the given value using its `Debug` formatting. @@ -540,10 +529,6 @@ fn log_link( &CoordinateFrame::update_fields().with_frame_id(link.name.clone()), )?; - let link_parent = urdf_tree - .get_parent_of_link(&link.name) - .map(|joint| &joint.child); - for (i, visual) in visual.iter().enumerate() { let urdf_rs::Visual { name, @@ -551,8 +536,8 @@ fn log_link( geometry, material, } = visual; - let name = name.clone().unwrap_or_else(|| format!("visual_{i}")); - let vis_entity = link_entity / EntityPathPart::new(name.clone()); + let visual_name = name.clone().unwrap_or_else(|| format!("visual_{i}")); + let visual_entity = link_entity / EntityPathPart::new(visual_name.clone()); // Prefer inline defined material properties if present, otherwise fall back to global material. let material = material.as_ref().and_then(|mat| { @@ -563,32 +548,33 @@ fn log_link( } }); - let link_child = urdf_rs::LinkName { link: name }; - // TODO send_transform( tx, store_id, - vis_entity.clone(), + visual_entity.clone(), origin, timepoint, - link_parent, - Some(&link_child), + link.name.clone(), + visual_name, )?; - if let Some(parent) = link_parent { - let coordinate_frame = - CoordinateFrame::update_fields().with_frame_id(parent.link.clone()); - send_archetype( - tx, - store_id, - vis_entity.clone(), - timepoint, - &coordinate_frame, - )?; - } + let coordinate_frame = CoordinateFrame::update_fields().with_frame_id(link.name.clone()); + send_archetype( + tx, + store_id, + visual_entity.clone(), + timepoint, + &coordinate_frame, + )?; log_geometry( - urdf_tree, tx, store_id, vis_entity, geometry, material, timepoint, + urdf_tree, + tx, + store_id, + visual_entity, + geometry, + material, + timepoint, )?; } @@ -598,31 +584,27 @@ fn log_link( origin, geometry, } = collision; - let name = name.clone().unwrap_or_else(|| format!("collision_{i}")); - let collision_entity = link_entity / EntityPathPart::new(name.clone()); + let collision_name = name.clone().unwrap_or_else(|| format!("collision_{i}")); + let collision_entity = link_entity / EntityPathPart::new(collision_name.clone()); - let link_child = urdf_rs::LinkName { link: name }; send_transform( tx, store_id, collision_entity.clone(), origin, timepoint, - link_parent, - Some(&link_child), + link.name.clone(), + collision_name, )?; - if let Some(parent) = link_parent { - let coordinate_frame = - CoordinateFrame::update_fields().with_frame_id(parent.link.clone()); - send_archetype( - tx, - store_id, - collision_entity.clone(), - timepoint, - &coordinate_frame, - )?; - } + let coordinate_frame = CoordinateFrame::update_fields().with_frame_id(link.name.clone()); + send_archetype( + tx, + store_id, + collision_entity.clone(), + timepoint, + &coordinate_frame, + )?; log_geometry( urdf_tree, From 964ce91dd4fea8439dbb1305f04f03b9e3a7cb41 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Thu, 27 Nov 2025 15:03:20 +0100 Subject: [PATCH 3/9] Log root frame identity (for now) --- crates/store/re_data_loader/src/loader_urdf.rs | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/crates/store/re_data_loader/src/loader_urdf.rs b/crates/store/re_data_loader/src/loader_urdf.rs index 9e411f00ed41..9c3e371d0ac6 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -6,6 +6,7 @@ use std::{ use ahash::{HashMap, HashMapExt as _, HashSet, HashSetExt as _}; use anyhow::{Context as _, bail}; use itertools::Itertools as _; +use re_log::warn; use urdf_rs::{Geometry, Joint, Link, Material, Robot, Vec3, Vec4}; use re_chunk::{ChunkBuilder, ChunkId, EntityPath, RowId, TimePoint}; @@ -290,6 +291,14 @@ fn log_robot( timepoint, &CoordinateFrame::update_fields().with_frame_id(urdf_tree.root.name.clone()), )?; + // TODO: Do we always need to do that? + send_archetype( + tx, + store_id, + entity_path.clone(), + timepoint, + &Transform3D::update_fields().with_child_frame(urdf_tree.root.name.clone()), + )?; walk_tree( &urdf_tree, From 2929ba446b7ee05318bfea28d8869e4efa4096fe Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Thu, 27 Nov 2025 15:05:33 +0100 Subject: [PATCH 4/9] remove unused import --- crates/store/re_data_loader/src/loader_urdf.rs | 1 - 1 file changed, 1 deletion(-) diff --git a/crates/store/re_data_loader/src/loader_urdf.rs b/crates/store/re_data_loader/src/loader_urdf.rs index 9c3e371d0ac6..a4857da52bd7 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -6,7 +6,6 @@ use std::{ use ahash::{HashMap, HashMapExt as _, HashSet, HashSetExt as _}; use anyhow::{Context as _, bail}; use itertools::Itertools as _; -use re_log::warn; use urdf_rs::{Geometry, Joint, Link, Material, Robot, Vec3, Vec4}; use re_chunk::{ChunkBuilder, ChunkId, EntityPath, RowId, TimePoint}; From c80baf5222d56cd575a8f76c451dc2e939af9755 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Fri, 28 Nov 2025 16:44:48 +0100 Subject: [PATCH 5/9] adapt to new name --- crates/store/re_data_loader/src/loader_urdf.rs | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/crates/store/re_data_loader/src/loader_urdf.rs b/crates/store/re_data_loader/src/loader_urdf.rs index a4857da52bd7..145dc8882ef6 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -288,7 +288,7 @@ fn log_robot( store_id, entity_path.clone(), timepoint, - &CoordinateFrame::update_fields().with_frame_id(urdf_tree.root.name.clone()), + &CoordinateFrame::update_fields().with_frame(urdf_tree.root.name.clone()), )?; // TODO: Do we always need to do that? send_archetype( @@ -378,7 +378,7 @@ fn log_joint( store_id, joint_path.clone(), timepoint, - &CoordinateFrame::update_fields().with_frame_id(parent.link.clone()), + &CoordinateFrame::update_fields().with_frame(parent.link.clone()), )?; // Send the joint origin, i.e. the default transform from parent link to child link. send_transform( @@ -534,7 +534,7 @@ fn log_link( store_id, link_entity.clone(), timepoint, - &CoordinateFrame::update_fields().with_frame_id(link.name.clone()), + &CoordinateFrame::update_fields().with_frame(link.name.clone()), )?; for (i, visual) in visual.iter().enumerate() { @@ -566,7 +566,7 @@ fn log_link( visual_name, )?; - let coordinate_frame = CoordinateFrame::update_fields().with_frame_id(link.name.clone()); + let coordinate_frame = CoordinateFrame::update_fields().with_frame(link.name.clone()); send_archetype( tx, store_id, @@ -605,7 +605,7 @@ fn log_link( collision_name, )?; - let coordinate_frame = CoordinateFrame::update_fields().with_frame_id(link.name.clone()); + let coordinate_frame = CoordinateFrame::update_fields().with_frame(link.name.clone()); send_archetype( tx, store_id, From 9d62a337cd6cd22983b0d20bd3f6b0bdc82b9e06 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 2 Dec 2025 09:32:57 +0100 Subject: [PATCH 6/9] don't log an implicit root connection --- crates/store/re_data_loader/src/loader_urdf.rs | 8 -------- 1 file changed, 8 deletions(-) diff --git a/crates/store/re_data_loader/src/loader_urdf.rs b/crates/store/re_data_loader/src/loader_urdf.rs index 145dc8882ef6..c6e38b4728bf 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -290,14 +290,6 @@ fn log_robot( timepoint, &CoordinateFrame::update_fields().with_frame(urdf_tree.root.name.clone()), )?; - // TODO: Do we always need to do that? - send_archetype( - tx, - store_id, - entity_path.clone(), - timepoint, - &Transform3D::update_fields().with_child_frame(urdf_tree.root.name.clone()), - )?; walk_tree( &urdf_tree, From 63f6a6ae85998a5df31fdb622e5ead632228e2db Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 2 Dec 2025 09:37:07 +0100 Subject: [PATCH 7/9] remove debug axis logging --- crates/store/re_data_loader/src/loader_urdf.rs | 8 -------- 1 file changed, 8 deletions(-) diff --git a/crates/store/re_data_loader/src/loader_urdf.rs b/crates/store/re_data_loader/src/loader_urdf.rs index c6e38b4728bf..1049029f75ab 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -454,14 +454,6 @@ fn send_transform( parent_frame: String, child_frame: String, ) -> anyhow::Result<()> { - // TODO: remove axis log this after debugging - send_archetype( - tx, - store_id, - entity_path.clone(), - timepoint, - &TransformAxes3D::update_fields().with_axis_length(0.1), - )?; send_archetype( tx, store_id, From 9770156575968f1f71170466585d855c77ace958 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 2 Dec 2025 09:51:00 +0100 Subject: [PATCH 8/9] Unused import --- crates/store/re_data_loader/src/loader_urdf.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crates/store/re_data_loader/src/loader_urdf.rs b/crates/store/re_data_loader/src/loader_urdf.rs index 1049029f75ab..02ddf5fa186c 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -12,7 +12,7 @@ use re_chunk::{ChunkBuilder, ChunkId, EntityPath, RowId, TimePoint}; use re_log_types::{EntityPathPart, StoreId}; use re_types::{ AsComponents, Component as _, ComponentDescriptor, SerializedComponentBatch, - archetypes::{Asset3D, CoordinateFrame, Transform3D, TransformAxes3D}, + archetypes::{Asset3D, CoordinateFrame, Transform3D}, datatypes::Vec3D, external::glam, }; From 60cf31ab096cce125181bcd4bb5eca26ada78afd Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 2 Dec 2025 10:28:11 +0100 Subject: [PATCH 9/9] Update animated_urdf (not working yet) --- examples/rust/animated_urdf/src/main.rs | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/examples/rust/animated_urdf/src/main.rs b/examples/rust/animated_urdf/src/main.rs index 64803b824033..2a8a198df392 100644 --- a/examples/rust/animated_urdf/src/main.rs +++ b/examples/rust/animated_urdf/src/main.rs @@ -20,7 +20,7 @@ fn main() -> anyhow::Result<()> { use clap::Parser as _; let args = Args::parse(); - let (rec, _serve_guard) = args.rerun.init("rerun_example_clock")?; + let (rec, _serve_guard) = args.rerun.init("rerun_example_animated_urdf")?; run(&rec, &args) } @@ -47,17 +47,16 @@ fn run(rec: &rerun::RecordingStream, _args: &Args) -> anyhow::Result<()> { joint.limit.lower..=joint.limit.upper, ); - // NOTE: each joint already has a fixed origin pose (logged with the URDF file), - // and Rerun won't allow us to override or add to that transform here. - // So instead we apply the dynamic rotation to the child link of the joint: - let child_link = urdf.get_joint_child(joint); - let link_path = urdf.get_link_path(child_link); + // Rerun loads the URDF transforms with child/parent frame relations. + // In order to move a joint, we just need to log a new transform between two of those frames. rec.log( - link_path, + "/transforms", &rerun::Transform3D::from_rotation(rerun::RotationAxisAngle::new( fixed_axis, dynamic_angle as f32, - )), + )) + .with_parent_frame(joint.parent.link.clone()) + .with_child_frame(joint.child.link.clone()), )?; } }