rapier3d-urdf: allow loading a robot at a non-zero initial joint configuration

[cogeor]

Hi! I'm building a Rust-based robotics project on top of rapier, and rapier3d-urdf has been really pleasant to work with. The recent additions (scale, mesh_converter, squeeze_empty_fixed_links) cover a lot of the awkward real-world URDF cases nicely.

I came across one limitation that's already flagged in the crate docs. It feels like something that belongs upstream rather than in everyone's downstream code, and since the docs explicitly invite contributions on it, I wanted to open an issue to discuss the right shape before sending a PR.

The gap

Right now a loaded robot always starts at its neutral pose, with every joint coordinate at 0. The crate docs call this out directly:

When inserting joints as multibody joints, they will be reset to their neutral position (all coordinates = 0).

And it's true for the impulse-joint path too: in urdf_to_joint, the child body is placed at pose1 * joint_to_parent, i.e. forward kinematics evaluated at q = 0, with no way to specify a different starting angle.

Why it matters

The neutral pose usually isn't where you actually want the robot to begin. Manipulators have a "home"/ready configuration, legged robots have a nominal stance, and a lot of RL/sim setups reset episodes to a specific posture.

When the controller's first target is a non-zero configuration but the bodies were spawned at q = 0, the first simulation step has to drive every joint from 0 to its setpoint at once. With stiff gains that shows up as a visible snap-and-recover at startup, and it corrupts the opening frames of anything you're recording (trajectories, camera/depth/segmentation capture, etc.). The usual workaround is to teleport every body after loading using hand-rolled FK, which is exactly the kind of thing this crate is meant to spare people from.

What I'd propose

A way to specify per-joint initial positions and have the loader place things accordingly. Rough shape (happy to adjust):

• A method on UrdfRobot, something like set_joint_positions(&mut self, positions: &HashMap<String, Real>), called after from_* and before insert_*. It walks the kinematic tree and recomputes link world-poses with the joint motion applied:
  • revolute/continuous: rotation by q about the joint axis,
  • prismatic: translation by q along the joint axis,
  • unspecified joints stay at 0.
• Optionally a convenience UrdfLoaderOptions::initial_joint_positions field that just feeds into the same code path, for people who'd rather configure it up front. (It can't fully live on the options struct since joint names aren't known until after parsing, so the method would be the real entry point.)

For the impulse-joint path this is just setting the body positions (which is what append_transform already does for a global shift, so there's precedent). For the multibody path the body poses are derived from joint coords, so the initial value would need to be set on the joint instead, which I think is the part most worth your input: MultibodyJoint currently exposes coords() but no setter, only apply_displacement. I'd rather agree on the intended mechanism than guess.

Where possible I'd reuse the existing core FK helpers (Multibody::forward_kinematics, RevoluteJoint::angle) rather than duplicate math.

A few open questions

1. Standalone set_joint_positions method, the UrdfLoaderOptions field, or both?
2. Unknown joint names in the map: silently ignore, or return an error?
3. Joints with limits: clamp to [lower, upper], debug-assert, or just trust the caller?
4. For multibody: is apply_displacement the path you'd want here, or would you be open to a small coordinate setter on MultibodyJoint/Multibody?
5. Worth noting the new rapier3d-mjcf loader (feat: add a mjcf loader #936) has the same gap, and MJCF treats initial configuration as a first-class concept (keyframes / qpos). If the FK-from-config helper lived somewhere shared, both loaders could use it. Let me know if you'd prefer I keep this URDF-local for now.

Offer

I'd love to contribute this upstream rather than keep carrying it in my own codebase. I've already worked through the approach locally (initial-config FK placement across the chain, for both the impulse and reduced-coordinate paths), so I'm fairly confident in the mechanics. I just wanted to settle the API direction and the multibody question with you first so the PR lands in a shape you're happy to merge.

Thanks again for the crate!