From 10d6a844fe1aa1944436d81a5dd038011ab73785 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Thu, 27 Nov 2025 11:07:44 +0100 Subject: [PATCH 01/13] wip --- .../store/re_data_loader/src/loader_urdf.rs | 124 ++++++++++++++++-- 1 file changed, 112 insertions(+), 12 deletions(-) diff --git a/crates/store/re_data_loader/src/loader_urdf.rs b/crates/store/re_data_loader/src/loader_urdf.rs index e8384c274ef8..64eb120a13d2 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -11,7 +11,7 @@ use re_sdk_types::archetypes::{Asset3D, Transform3D}; use re_sdk_types::datatypes::Vec3D; use re_sdk_types::external::glam; use re_sdk_types::{AsComponents, Component as _, ComponentDescriptor, SerializedComponentBatch}; -use urdf_rs::{Geometry, Joint, Link, Material, Robot, Vec3, Vec4}; +use urdf_rs::{Geometry, Joint, Link, LinkName, Material, Robot, Vec3, Vec4}; use crate::{DataLoader, DataLoaderError, LoadedData}; @@ -278,6 +278,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(urdf_tree.root.name.clone()), + )?; + walk_tree( &urdf_tree, tx, @@ -341,8 +350,8 @@ fn log_joint( name: _, joint_type, origin, - parent: _, - child: _, + parent, + child, axis, limit, calibration, @@ -351,7 +360,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(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, @@ -400,13 +426,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( @@ -415,6 +451,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]; @@ -422,12 +460,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), ) } } @@ -481,6 +527,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(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, @@ -489,7 +548,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| { @@ -500,7 +559,28 @@ 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(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, @@ -514,9 +594,29 @@ 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(parent.link.clone()); + send_archetype( + tx, + store_id, + collision_entity.clone(), + timepoint, + &coordinate_frame, + )?; + } log_geometry( urdf_tree, From 48eabad5cbc7c09db17a5f9399859a9dfd27a5cd Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Thu, 27 Nov 2025 12:55:38 +0100 Subject: [PATCH 02/13] Clean up a bit --- .../store/re_data_loader/src/loader_urdf.rs | 133 ++++++++---------- 1 file changed, 59 insertions(+), 74 deletions(-) diff --git a/crates/store/re_data_loader/src/loader_urdf.rs b/crates/store/re_data_loader/src/loader_urdf.rs index 64eb120a13d2..88eb517dcbde 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -5,6 +5,7 @@ use std::sync::mpsc::Sender; use ahash::{HashMap, HashMapExt as _, HashSet, HashSetExt as _}; use anyhow::{Context as _, bail}; use itertools::Itertools as _; + use re_chunk::{ChunkBuilder, ChunkId, EntityPath, RowId, TimePoint}; use re_log_types::{EntityPathPart, StoreId}; use re_sdk_types::archetypes::{Asset3D, Transform3D}; @@ -278,7 +279,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, @@ -375,8 +376,8 @@ fn log_joint( joint_path.clone(), origin, timepoint, - Some(parent), - Some(child), + parent.link.clone(), + child.link.clone(), )?; log_debug_format( @@ -428,21 +429,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( @@ -451,31 +448,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. @@ -536,10 +526,6 @@ fn log_link( &CoordinateFrame::update_fields().with_frame(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, @@ -547,8 +533,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| { @@ -559,31 +545,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(parent.link.clone()); - send_archetype( - tx, - store_id, - vis_entity.clone(), - timepoint, - &coordinate_frame, - )?; - } + let coordinate_frame = CoordinateFrame::update_fields().with_frame(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, )?; } @@ -593,30 +581,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(parent.link.clone()); - send_archetype( - tx, - store_id, - collision_entity.clone(), - timepoint, - &coordinate_frame, - )?; - } + let coordinate_frame = CoordinateFrame::update_fields().with_frame(link.name.clone()); + send_archetype( + tx, + store_id, + collision_entity.clone(), + timepoint, + &coordinate_frame, + )?; log_geometry( urdf_tree, From ddd4848beb7b9e5b56e985cce4636855f2139a80 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Thu, 27 Nov 2025 15:03:20 +0100 Subject: [PATCH 03/13] Log root frame identity (for now) --- crates/store/re_data_loader/src/loader_urdf.rs | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/crates/store/re_data_loader/src/loader_urdf.rs b/crates/store/re_data_loader/src/loader_urdf.rs index 88eb517dcbde..8fd20684da43 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -287,6 +287,14 @@ 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 19fc26f0eca34ba83bcb58f509509aa170a59698 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 2 Dec 2025 09:32:57 +0100 Subject: [PATCH 04/13] 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 8fd20684da43..88eb517dcbde 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -287,14 +287,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 45acd18b0eaddc74a83f0a9bec92fc9ac31fc768 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 2 Dec 2025 09:37:07 +0100 Subject: [PATCH 05/13] 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 88eb517dcbde..679225c65a58 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -451,14 +451,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 873a56361a31fddbb4aa94c12e545d76ecafd359 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 2 Dec 2025 10:28:11 +0100 Subject: [PATCH 06/13] 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 b0384e769635..8036e6e5b656 100644 --- a/examples/rust/animated_urdf/src/main.rs +++ b/examples/rust/animated_urdf/src/main.rs @@ -21,7 +21,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) } @@ -48,17 +48,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()), )?; } } From 578c53e62091bd15e15e82b1ba7e3b85208dcb5e Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Thu, 4 Dec 2025 13:39:12 +0100 Subject: [PATCH 07/13] more import junk --- 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 679225c65a58..86a60f7dfffe 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -5,7 +5,6 @@ use std::sync::mpsc::Sender; use ahash::{HashMap, HashMapExt as _, HashSet, HashSetExt as _}; use anyhow::{Context as _, bail}; use itertools::Itertools as _; - use re_chunk::{ChunkBuilder, ChunkId, EntityPath, RowId, TimePoint}; use re_log_types::{EntityPathPart, StoreId}; use re_sdk_types::archetypes::{Asset3D, Transform3D}; From 5f348ea38d1287e8a4d7f207c4714fc6d64ec775 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Thu, 4 Dec 2025 13:59:45 +0100 Subject: [PATCH 08/13] Make sure that visual/collision meshes have unique frame IDs --- .../store/re_data_loader/src/loader_urdf.rs | 24 +++++++++++++------ 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/crates/store/re_data_loader/src/loader_urdf.rs b/crates/store/re_data_loader/src/loader_urdf.rs index 86a60f7dfffe..0f16efe7ca52 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -509,12 +509,13 @@ fn log_link( )?; // Log coordinate frame ID of the link. + let link_name = link.name.clone(); send_archetype( tx, store_id, link_entity.clone(), timepoint, - &CoordinateFrame::update_fields().with_frame(link.name.clone()), + &CoordinateFrame::update_fields().with_frame(link_name.clone()), )?; for (i, visual) in visual.iter().enumerate() { @@ -527,6 +528,10 @@ fn log_link( let visual_name = name.clone().unwrap_or_else(|| format!("visual_{i}")); let visual_entity = link_entity / EntityPathPart::new(visual_name.clone()); + // We have to make sure that the frame ID is unique, otherwise we might end up with multiple "visual_0" etc. + // Prefix with the link name. + let visual_frame_id = format!("{link_name}_{visual_name}"); + // Prefer inline defined material properties if present, otherwise fall back to global material. let material = material.as_ref().and_then(|mat| { if mat.color.is_some() || mat.texture.is_some() { @@ -542,11 +547,11 @@ fn log_link( visual_entity.clone(), origin, timepoint, - link.name.clone(), - visual_name, + link_name.clone(), + visual_frame_id.clone(), )?; - let coordinate_frame = CoordinateFrame::update_fields().with_frame(link.name.clone()); + let coordinate_frame = CoordinateFrame::update_fields().with_frame(visual_frame_id.clone()); send_archetype( tx, store_id, @@ -575,17 +580,22 @@ fn log_link( let collision_name = name.clone().unwrap_or_else(|| format!("collision_{i}")); let collision_entity = link_entity / EntityPathPart::new(collision_name.clone()); + // We have to make sure that the frame ID is unique, otherwise we might end up with multiple "collision_0" etc. + // Prefix with the link name. + let collision_frame_id = format!("{link_name}_{collision_name}"); + send_transform( tx, store_id, collision_entity.clone(), origin, timepoint, - link.name.clone(), - collision_name, + link_name.clone(), + collision_frame_id.clone(), )?; - let coordinate_frame = CoordinateFrame::update_fields().with_frame(link.name.clone()); + let coordinate_frame = + CoordinateFrame::update_fields().with_frame(collision_frame_id.clone()); send_archetype( tx, store_id, From 45333af5c01d812300efafaccefff2e6175b99bd Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Thu, 4 Dec 2025 14:33:40 +0100 Subject: [PATCH 09/13] improve comment wording --- crates/store/re_data_loader/src/loader_urdf.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/crates/store/re_data_loader/src/loader_urdf.rs b/crates/store/re_data_loader/src/loader_urdf.rs index 0f16efe7ca52..c75548856334 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -528,8 +528,8 @@ fn log_link( let visual_name = name.clone().unwrap_or_else(|| format!("visual_{i}")); let visual_entity = link_entity / EntityPathPart::new(visual_name.clone()); - // We have to make sure that the frame ID is unique, otherwise we might end up with multiple "visual_0" etc. - // Prefix with the link name. + // Prefix with the link name, otherwise we might end up with multiple coordinate frames named "visual_0" etc. + // Note that this doesn't apply to the entity path part, there it's fine to have e.g. /base/visual_0 and /base/link1/visual_0. let visual_frame_id = format!("{link_name}_{visual_name}"); // Prefer inline defined material properties if present, otherwise fall back to global material. @@ -580,8 +580,8 @@ fn log_link( let collision_name = name.clone().unwrap_or_else(|| format!("collision_{i}")); let collision_entity = link_entity / EntityPathPart::new(collision_name.clone()); - // We have to make sure that the frame ID is unique, otherwise we might end up with multiple "collision_0" etc. - // Prefix with the link name. + // Prefix with the link name, otherwise we might end up with multiple coordinate frames named "collision_0" etc. + // Note that this doesn't apply to the entity path part, there it's fine to have e.g. /base/collision_0 and /base/link1/collision_0. let collision_frame_id = format!("{link_name}_{collision_name}"); send_transform( From 33d2b6fc9b02b8297ad3ba5133f553101493af9f Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Thu, 4 Dec 2025 16:22:51 +0100 Subject: [PATCH 10/13] set translation in animated_urdf --- examples/rust/animated_urdf/src/main.rs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/rust/animated_urdf/src/main.rs b/examples/rust/animated_urdf/src/main.rs index 8036e6e5b656..277d897b8f7c 100644 --- a/examples/rust/animated_urdf/src/main.rs +++ b/examples/rust/animated_urdf/src/main.rs @@ -12,6 +12,7 @@ struct Args { rerun: rerun::clap::RerunArgs, } +use rerun::components::Translation3D; use rerun::external::re_data_loader::UrdfTree; use rerun::external::{re_log, urdf_rs}; @@ -56,6 +57,7 @@ fn run(rec: &rerun::RecordingStream, _args: &Args) -> anyhow::Result<()> { fixed_axis, dynamic_angle as f32, )) + .with_translation(Translation3D::from(joint.origin.xyz.0)) .with_parent_frame(joint.parent.link.clone()) .with_child_frame(joint.child.link.clone()), )?; From 3575a3ad7b3cfde69e7317ffe1be502afdc48c3a Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Thu, 4 Dec 2025 17:02:54 +0100 Subject: [PATCH 11/13] fix animated_urdf (log full transforms, not just updates) --- Cargo.lock | 1 + examples/rust/animated_urdf/Cargo.toml | 1 + examples/rust/animated_urdf/src/main.rs | 21 ++++++++++++++++++--- 3 files changed, 20 insertions(+), 3 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 73c0c5cb1a6d..f262b0c591df 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -275,6 +275,7 @@ dependencies = [ "anyhow", "clap", "emath", + "glam", "rerun", ] diff --git a/examples/rust/animated_urdf/Cargo.toml b/examples/rust/animated_urdf/Cargo.toml index ef54f21a5855..b8afb473e6e9 100644 --- a/examples/rust/animated_urdf/Cargo.toml +++ b/examples/rust/animated_urdf/Cargo.toml @@ -12,3 +12,4 @@ rerun = { path = "../../../crates/top/rerun", features = ["clap", "data_loaders" anyhow.workspace = true clap = { workspace = true, features = ["derive"] } emath.workspace = true +glam.workspace = true diff --git a/examples/rust/animated_urdf/src/main.rs b/examples/rust/animated_urdf/src/main.rs index 277d897b8f7c..2cea3a02a8c8 100644 --- a/examples/rust/animated_urdf/src/main.rs +++ b/examples/rust/animated_urdf/src/main.rs @@ -49,13 +49,28 @@ fn run(rec: &rerun::RecordingStream, _args: &Args) -> anyhow::Result<()> { joint.limit.lower..=joint.limit.upper, ); + // Compute the full rotation for this joint. + // TODO(michael): we could make this a bit nicer with a better URDF utility. + let rotation = glam::Quat::from_euler( + glam::EulerRot::XYZ, + joint.origin.rpy[0] as f32, + joint.origin.rpy[1] as f32, + joint.origin.rpy[2] as f32, + ) * glam::Quat::from_axis_angle( + glam::Vec3::new( + fixed_axis[0] as f32, + fixed_axis[1] as f32, + fixed_axis[2] as f32, + ), + dynamic_angle as f32, + ); + // 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( "/transforms", - &rerun::Transform3D::from_rotation(rerun::RotationAxisAngle::new( - fixed_axis, - dynamic_angle as f32, + &rerun::Transform3D::from_rotation(rerun::Quaternion::from_xyzw( + rotation.to_array(), )) .with_translation(Translation3D::from(joint.origin.xyz.0)) .with_parent_frame(joint.parent.link.clone()) From 78db908462d358d91d8070713ad0e7361757ef02 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Thu, 11 Dec 2025 11:03:52 +0100 Subject: [PATCH 12/13] another rebase fix --- crates/store/re_data_loader/src/loader_urdf.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crates/store/re_data_loader/src/loader_urdf.rs b/crates/store/re_data_loader/src/loader_urdf.rs index c75548856334..6d25d97152b5 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -7,11 +7,11 @@ use anyhow::{Context as _, bail}; use itertools::Itertools as _; use re_chunk::{ChunkBuilder, ChunkId, EntityPath, RowId, TimePoint}; use re_log_types::{EntityPathPart, StoreId}; -use re_sdk_types::archetypes::{Asset3D, Transform3D}; +use re_sdk_types::archetypes::{Asset3D, CoordinateFrame, Transform3D}; use re_sdk_types::datatypes::Vec3D; use re_sdk_types::external::glam; use re_sdk_types::{AsComponents, Component as _, ComponentDescriptor, SerializedComponentBatch}; -use urdf_rs::{Geometry, Joint, Link, LinkName, Material, Robot, Vec3, Vec4}; +use urdf_rs::{Geometry, Joint, Link, Material, Robot, Vec3, Vec4}; use crate::{DataLoader, DataLoaderError, LoadedData}; From bd8156665b33a2b66673233aebb31cbd368df150 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Thu, 11 Dec 2025 11:25:24 +0100 Subject: [PATCH 13/13] Attach visual & collision with instance poses --- .../store/re_data_loader/src/loader_urdf.rs | 76 ++++++++++--------- 1 file changed, 42 insertions(+), 34 deletions(-) diff --git a/crates/store/re_data_loader/src/loader_urdf.rs b/crates/store/re_data_loader/src/loader_urdf.rs index 6d25d97152b5..03b3d7dd2a01 100644 --- a/crates/store/re_data_loader/src/loader_urdf.rs +++ b/crates/store/re_data_loader/src/loader_urdf.rs @@ -7,7 +7,7 @@ use anyhow::{Context as _, bail}; use itertools::Itertools as _; use re_chunk::{ChunkBuilder, ChunkId, EntityPath, RowId, TimePoint}; use re_log_types::{EntityPathPart, StoreId}; -use re_sdk_types::archetypes::{Asset3D, CoordinateFrame, Transform3D}; +use re_sdk_types::archetypes::{Asset3D, CoordinateFrame, InstancePoses3D, Transform3D}; use re_sdk_types::datatypes::Vec3D; use re_sdk_types::external::glam; use re_sdk_types::{AsComponents, Component as _, ComponentDescriptor, SerializedComponentBatch}; @@ -441,6 +441,15 @@ fn transform_from_pose( .with_child_frame(child_frame) } +fn instance_poses_from_pose(origin: &urdf_rs::Pose) -> InstancePoses3D { + let urdf_rs::Pose { xyz, rpy } = origin; + let translation = Vec3D::new(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); + InstancePoses3D::update_fields() + .with_translations(vec![translation]) + .with_quaternions(vec![quaternion]) +} + fn send_transform( tx: &Sender, store_id: &StoreId, @@ -459,6 +468,30 @@ fn send_transform( ) } +fn send_instance_pose_with_frame( + tx: &Sender, + store_id: &StoreId, + entity_path: EntityPath, + timepoint: &TimePoint, + origin: &urdf_rs::Pose, + parent_frame: String, +) -> anyhow::Result<()> { + send_archetype( + tx, + store_id, + entity_path.clone(), + timepoint, + &instance_poses_from_pose(origin), + )?; + send_archetype( + tx, + store_id, + entity_path, + timepoint, + &CoordinateFrame::update_fields().with_frame(parent_frame), + ) +} + /// Log the given value using its `Debug` formatting. /// /// TODO(#402): support dynamic structured logging @@ -528,10 +561,6 @@ fn log_link( let visual_name = name.clone().unwrap_or_else(|| format!("visual_{i}")); let visual_entity = link_entity / EntityPathPart::new(visual_name.clone()); - // Prefix with the link name, otherwise we might end up with multiple coordinate frames named "visual_0" etc. - // Note that this doesn't apply to the entity path part, there it's fine to have e.g. /base/visual_0 and /base/link1/visual_0. - let visual_frame_id = format!("{link_name}_{visual_name}"); - // Prefer inline defined material properties if present, otherwise fall back to global material. let material = material.as_ref().and_then(|mat| { if mat.color.is_some() || mat.texture.is_some() { @@ -541,23 +570,15 @@ fn log_link( } }); - send_transform( + // A visual geometry has no frame ID of its own and has a constant pose, + // so we attach it to the link using an instance pose. + send_instance_pose_with_frame( tx, store_id, visual_entity.clone(), - origin, timepoint, + origin, link_name.clone(), - visual_frame_id.clone(), - )?; - - let coordinate_frame = CoordinateFrame::update_fields().with_frame(visual_frame_id.clone()); - send_archetype( - tx, - store_id, - visual_entity.clone(), - timepoint, - &coordinate_frame, )?; log_geometry( @@ -580,28 +601,15 @@ fn log_link( let collision_name = name.clone().unwrap_or_else(|| format!("collision_{i}")); let collision_entity = link_entity / EntityPathPart::new(collision_name.clone()); - // Prefix with the link name, otherwise we might end up with multiple coordinate frames named "collision_0" etc. - // Note that this doesn't apply to the entity path part, there it's fine to have e.g. /base/collision_0 and /base/link1/collision_0. - let collision_frame_id = format!("{link_name}_{collision_name}"); - - send_transform( + // A collision geometry has no frame ID of its own and has a constant pose, + // so we attach it to the link using an instance pose. + send_instance_pose_with_frame( tx, store_id, collision_entity.clone(), - origin, timepoint, + origin, link_name.clone(), - collision_frame_id.clone(), - )?; - - let coordinate_frame = - CoordinateFrame::update_fields().with_frame(collision_frame_id.clone()); - send_archetype( - tx, - store_id, - collision_entity.clone(), - timepoint, - &coordinate_frame, )?; log_geometry(