From e92bfcafbf8225025694ce61b2304c1fc6fab2c4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Mon, 7 Sep 2026 13:48:46 +0200 Subject: [PATCH] fix(urdf): compose origin rpy as fixed-axis roll-pitch-yaw --- crates/rapier3d-urdf/CHANGELOG.md | 10 ++ crates/rapier3d-urdf/src/lib.rs | 12 +- .../tests/urdf_rpy_convention.rs | 132 ++++++++++++++++++ 3 files changed, 150 insertions(+), 4 deletions(-) create mode 100644 crates/rapier3d-urdf/tests/urdf_rpy_convention.rs diff --git a/crates/rapier3d-urdf/CHANGELOG.md b/crates/rapier3d-urdf/CHANGELOG.md index bf46b1f43..adbf660e3 100644 --- a/crates/rapier3d-urdf/CHANGELOG.md +++ b/crates/rapier3d-urdf/CHANGELOG.md @@ -1,3 +1,13 @@ +## Unreleased + +### Fix + +- URDF `` angles are now composed as fixed-axis roll-pitch-yaw + (`Rz(yaw) * Ry(pitch) * Rx(roll)`), as the URDF specification defines. They + were composed as intrinsic XYZ Euler angles, which only agrees when at most + one angle is non-zero and misplaced every link downstream of an origin such + as `rpy="-1.5708 -1.5708 0"`. + ## 0.4.0 ### Modified diff --git a/crates/rapier3d-urdf/src/lib.rs b/crates/rapier3d-urdf/src/lib.rs index 103ba26d8..dd57aeaef 100644 --- a/crates/rapier3d-urdf/src/lib.rs +++ b/crates/rapier3d-urdf/src/lib.rs @@ -788,6 +788,9 @@ fn urdf_to_colliders( .collect() } +/// Converts a URDF `` into a pose. URDF `rpy` are fixed-axis +/// roll-pitch-yaw angles: the rotation is `Rz(yaw) * Ry(pitch) * Rx(roll)`, +/// which is glam's intrinsic `ZYX` sequence taken as (yaw, pitch, roll). fn urdf_to_pose(pose: &UrdfPose) -> Pose { Pose::from_parts( Vector::new( @@ -796,10 +799,10 @@ fn urdf_to_pose(pose: &UrdfPose) -> Pose { pose.xyz[2] as Real, ), Rotation::from_euler( - EulerRot::XYZ, - pose.rpy[0] as Real, - pose.rpy[1] as Real, + EulerRot::ZYX, pose.rpy[2] as Real, + pose.rpy[1] as Real, + pose.rpy[0] as Real, ), ) } @@ -945,8 +948,9 @@ fn is_link_empty(link: &urdf_rs::Link) -> bool { && inertia.izz == 0.0 } +/// The inverse of [`urdf_to_pose`]: fixed-axis roll-pitch-yaw from a pose. fn pose_to_urdf_pose(pose: &Pose) -> UrdfPose { - let (rx, ry, rz) = pose.rotation.to_euler(EulerRot::XYZ); + let (rz, ry, rx) = pose.rotation.to_euler(EulerRot::ZYX); UrdfPose { xyz: urdf_rs::Vec3([ pose.translation.x as f64, diff --git a/crates/rapier3d-urdf/tests/urdf_rpy_convention.rs b/crates/rapier3d-urdf/tests/urdf_rpy_convention.rs new file mode 100644 index 000000000..a5b9ef258 --- /dev/null +++ b/crates/rapier3d-urdf/tests/urdf_rpy_convention.rs @@ -0,0 +1,132 @@ +//! URDF `` angles are fixed-axis roll-pitch-yaw: the +//! rotation is `Rz(yaw) * Ry(pitch) * Rx(roll)`. The loader used to compose +//! them as intrinsic XYZ (`Rx * Ry * Rz`), which only agrees when at most one +//! angle is non-zero; the SO-101's `shoulder_lift` origin (`rpy="-1.5708 +//! -1.5708 0"`) put every downstream link in the wrong place. + +use rapier3d::prelude::*; +use rapier3d_urdf::{UrdfLoaderOptions, UrdfRobot}; +use std::path::Path; + +const URDF: &str = r#" + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +"#; + +/// `Rz(yaw) * Ry(pitch) * Rx(roll)`, the URDF specification's convention. +fn urdf_rotation(rpy: [f32; 3]) -> Rotation { + Rotation::from_rotation_z(rpy[2]) + * Rotation::from_rotation_y(rpy[1]) + * Rotation::from_rotation_x(rpy[0]) +} + +fn assert_pose_eq(actual: &Pose, expected: &Pose, what: &str) { + assert!( + (actual.translation - expected.translation).length() < 1.0e-5, + "{what}: translation {:?} != {:?}", + actual.translation, + expected.translation + ); + assert!( + actual.rotation.angle_between(expected.rotation) < 1.0e-4, + "{what}: rotation {:?} != {:?}", + actual.rotation, + expected.rotation + ); +} + +#[test] +fn link_poses_follow_fixed_axis_rpy() { + let (robot, _) = UrdfRobot::from_str(URDF, UrdfLoaderOptions::default(), Path::new(".")) + .expect("URDF parses"); + let lift = Pose::from_parts( + Vector::new(-0.03, -0.018, -0.054), + urdf_rotation([-1.5708, -1.5708, 0.0]), + ); + let flex = Pose::from_parts( + Vector::new(-0.11, -0.028, 0.0), + urdf_rotation([0.0, 0.0, 1.5708]), + ); + assert_pose_eq(robot.links[1].body.position(), &lift, "arm link"); + assert_pose_eq(robot.links[2].body.position(), &(lift * flex), "tip link"); +} + +#[test] +fn collider_origin_follows_fixed_axis_rpy() { + let (robot, _) = UrdfRobot::from_str(URDF, UrdfLoaderOptions::default(), Path::new(".")) + .expect("URDF parses"); + let expected = Pose::from_parts(Vector::new(0.1, 0.2, 0.3), urdf_rotation([0.4, 0.5, 0.6])); + let collider = &robot.links[1].colliders[0].collider; + assert_pose_eq(collider.position(), &expected, "arm collider"); +} + +#[test] +fn multibody_forward_kinematics_matches_the_origins() { + let (robot, _) = UrdfRobot::from_str(URDF, UrdfLoaderOptions::default(), Path::new(".")) + .expect("URDF parses"); + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + let handles = robot.insert_using_multibody_joints( + &mut bodies, + &mut colliders, + &mut multibody_joints, + rapier3d_urdf::UrdfMultibodyOptions::empty(), + ); + let tip = handles.links[2].body; + let link = *multibody_joints + .rigid_body_link(tip) + .expect("tip is a multibody link"); + let mb = multibody_joints.get_multibody_mut(link.multibody).unwrap(); + // Link poses are derived from the joint frames by forward kinematics. + mb.forward_kinematics(&bodies, false); + let lift = Pose::from_parts( + Vector::new(-0.03, -0.018, -0.054), + urdf_rotation([-1.5708, -1.5708, 0.0]), + ); + let flex = Pose::from_parts( + Vector::new(-0.11, -0.028, 0.0), + urdf_rotation([0.0, 0.0, 1.5708]), + ); + assert_pose_eq( + mb.link(link.id).unwrap().local_to_world(), + &(lift * flex), + "tip FK", + ); +}