dimforge / dimforge/rapier

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

Open
#942 0 comments 0 reactions 0 assignees View on GitHub
Dominant language
Rust
Stars
5.7k
Forks
387
Avg merge
4d 23h
Merged PRs (30d)
6

Description

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)`, 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 (#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!

Contributor guide

Open the contributing guide

Assessment

This issue has not been assessed yet.

Get new issues in your inbox

A short digest of beginner-friendly GitHub issues.