Skip to content

Fix joint-order-independent FK#2283

Open
bobboli wants to merge 3 commits intonewton-physics:mainfrom
bobboli:bobboli/fix-joint-eval-order
Open

Fix joint-order-independent FK#2283
bobboli wants to merge 3 commits intonewton-physics:mainfrom
bobboli:bobboli/fix-joint-eval-order

Conversation

@bobboli
Copy link
Copy Markdown
Contributor

@bobboli bobboli commented Mar 31, 2026

Description

Fix eval_fk for articulations whose joints are stored out of topological order.

This PR introduces a per-articulation joint_eval_order array that preserves
stable public joint indices while letting FK evaluate joints parent-before-child
internally. It also adds regressions covering FK equivalence and FK->IK
round-tripping for an out-of-order articulation.

Closes #910.

Checklist

  • New or existing tests cover these changes
  • The documentation is up to date with these changes
  • CHANGELOG.md has been updated (if user-facing change)

Test plan

uv run python -m unittest   newton.tests.test_model.TestModelJoints.test_eval_fk_handles_out_of_order_joints   newton.tests.test_model.TestModelJoints.test_eval_ik_handles_out_of_order_joints

uv run python -m unittest newton.tests.test_control_force
uvx pre-commit run -a

Summary by CodeRabbit

  • New Features

    • FK now uses a per-joint evaluation order so articulations produce correct body transforms even if joints were created in any sequence; kinematics (FK/IK) handle out-of-order joints reliably.
  • Tests

    • Added tests that validate forward and inverse kinematics remain correct and recover joint states regardless of joint creation order.

@coderabbitai
Copy link
Copy Markdown
Contributor

coderabbitai bot commented Mar 31, 2026

No actionable comments were generated in the recent review. 🎉

ℹ️ Recent review info
⚙️ Run configuration

Configuration used: Path: .coderabbit.yml

Review profile: CHILL

Plan: Pro

Run ID: 524cc6d4-394b-4c85-a98a-7f0303ed2253

📥 Commits

Reviewing files that changed from the base of the PR and between d5a3874 and eb1cf26.

📒 Files selected for processing (1)
  • newton/_src/sim/articulation.py
🚧 Files skipped from review as they are similar to previous changes (1)
  • newton/_src/sim/articulation.py

📝 Walkthrough

Walkthrough

Adds a per-articulation topological joint evaluation order to Model (computed in the builder) and threads it through FK/IK kernels (including batched paths) so joint-indexed accesses use the topological order instead of declaration order.

Changes

Cohort / File(s) Summary
FK kernel & host launch
newton/_src/sim/articulation.py
Added joint_eval_order parameter to eval_single_articulation_fk and eval_articulation_fk; remapped joint-indexed reads to use joint_eval_order[j]; updated eval_fk host launch to pass model.joint_eval_order.
Model builder: compute order
newton/_src/sim/builder.py
During ModelBuilder.finalize() compute per-articulation joint edge lists and run topological_sort(..., use_dfs=True, custom_indices=art_joints); fill global joint_eval_order slice per articulation; set m.joint_eval_order = wp.array(..., dtype=wp.int32); re-raise errors with articulation context.
Model attribute & registration
newton/_src/sim/model.py
Added `joint_eval_order: wp.array(dtype=wp.int32)
Batched IK/FK path
newton/_src/sim/ik/ik_common.py
Threaded joint_eval_order into _eval_fk_articulation_batched and updated eval_fk_batched kernel launches to include model.joint_eval_order.
Tests
newton/tests/test_model.py
Added test_eval_fk_handles_out_of_order_joints and test_eval_ik_handles_out_of_order_joints to verify FK/IK correctness when joints are declared out-of-order.

Sequence Diagram(s)

sequenceDiagram
    participant Builder as Builder
    participant Model as Model
    participant Host as Host
    participant FKKernel as FK Kernel
    Builder->>Builder: build per-articulation edge lists\nrun topological_sort per articulation
    Builder->>Model: set m.joint_eval_order (wp.array int32)
    Host->>Model: read model.joint_eval_order
    Host->>FKKernel: launch eval_articulation_fk(..., joint_eval_order)
    FKKernel->>FKKernel: for i in 0..num_joints-1\n  joint_idx = joint_eval_order[i]\n  read joint props via joint_idx\n  compute transforms/velocities
    FKKernel->>Host: write body transforms/velocities
Loading

Estimated code review effort

🎯 3 (Moderate) | ⏱️ ~25 minutes

Possibly related PRs

Suggested reviewers

  • mzamoramora-nvidia
  • eric-heiden
🚥 Pre-merge checks | ✅ 4 | ❌ 1

❌ Failed checks (1 warning)

Check name Status Explanation Resolution
Docstring Coverage ⚠️ Warning Docstring coverage is 33.33% which is insufficient. The required threshold is 80.00%. Write docstrings for the functions missing them to satisfy the coverage threshold.
✅ Passed checks (4 passed)
Check name Status Explanation
Description Check ✅ Passed Check skipped - CodeRabbit’s high-level summary is enabled.
Title check ✅ Passed The title 'Fix joint-order-independent FK' directly addresses the main objective of making eval_fk independent of joint ordering, which is the core focus of all code changes in this PR.
Linked Issues check ✅ Passed The PR fully implements the solution for issue #910 by introducing joint_eval_order array to enable topological joint traversal while preserving stable public joint indices, with comprehensive test coverage for FK equivalence and FK-IK round-tripping.
Out of Scope Changes check ✅ Passed All changes are directly aligned with issue #910 objectives: introducing joint_eval_order computation in builder.py, using it in articulation.py FK kernels, updating model.py with new field, extending IK support, and adding regression tests; no unrelated modifications detected.

✏️ Tip: You can configure your own custom pre-merge checks in the settings.

✨ Finishing Touches
🧪 Generate unit tests (beta)
  • Create PR with unit tests

Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out.

❤️ Share

Comment @coderabbitai help to get the list of available commands and usage tips.

@bobboli bobboli had a problem deploying to external-pr-approval March 31, 2026 17:49 — with GitHub Actions Error
@bobboli bobboli had a problem deploying to external-pr-approval March 31, 2026 17:49 — with GitHub Actions Error
Add a per-articulation topological evaluation order so kinematic
kernels no longer depend on storage order within an articulation.
This keeps public joint indices stable while making eval_fk and the
batched FK path robust to out-of-order joints.

Add regressions covering FK equivalence and IK round-tripping for an
articulation whose joints are stored out of parent-before-child order.
@bobboli bobboli force-pushed the bobboli/fix-joint-eval-order branch from c768b2f to 50f943c Compare March 31, 2026 18:03
@bobboli bobboli had a problem deploying to external-pr-approval March 31, 2026 18:03 — with GitHub Actions Error
@bobboli bobboli had a problem deploying to external-pr-approval March 31, 2026 18:03 — with GitHub Actions Error
Copy link
Copy Markdown
Contributor

@coderabbitai coderabbitai bot left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Actionable comments posted: 1

🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.

Inline comments:
In `@newton/_src/sim/articulation.py`:
- Around line 207-211: The lines that compute X_wpj at the top (X_wpj = X_pj; if
parent >= 0: X_wp = body_q[parent]; X_wpj = X_wp * X_wpj) are dead — X_wpj is
overwritten later and never used in the joint-type dispatch — so remove that
initial computation; delete the X_wpj initialization and the conditional block
referencing parent and body_q to avoid the duplicated/unused transform and rely
on the later correct computation (referencing X_wpj, X_pj, parent, body_q).
🪄 Autofix (Beta)

Fix all unresolved CodeRabbit comments on this PR:

  • Push a commit to this branch (recommended)
  • Create a new PR with the fixes

ℹ️ Review info
⚙️ Run configuration

Configuration used: Path: .coderabbit.yml

Review profile: CHILL

Plan: Pro

Run ID: 3d2d3cb7-cb63-4d83-8381-02c7f996159b

📥 Commits

Reviewing files that changed from the base of the PR and between 2684d75 and 50f943c.

📒 Files selected for processing (5)
  • newton/_src/sim/articulation.py
  • newton/_src/sim/builder.py
  • newton/_src/sim/ik/ik_common.py
  • newton/_src/sim/model.py
  • newton/tests/test_model.py

Comment thread newton/_src/sim/articulation.py Outdated
Drop the duplicated X_wpj setup at the top of eval_single_articulation_fk.
It is overwritten later before use, so removing it keeps the FK path
clear and addresses the review comment without changing behavior.
@bobboli bobboli had a problem deploying to external-pr-approval April 1, 2026 13:26 — with GitHub Actions Error
@bobboli bobboli had a problem deploying to external-pr-approval April 1, 2026 13:26 — with GitHub Actions Error
Copy link
Copy Markdown
Contributor

@coderabbitai coderabbitai bot left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Caution

Some comments are outside the diff and can’t be posted inline due to platform limitations.

⚠️ Outside diff range comments (1)
newton/_src/sim/articulation.py (1)

202-255: ⚠️ Potential issue | 🟡 Minor

Rename local type to avoid shadowing Python builtin.

type shadows the builtin type() (Ruff A001). Rename to joint_type_id and update all 6 comparisons within the method.

♻️ Proposed fix
-        type = joint_type[joint_index]
+        joint_type_id = joint_type[joint_index]
@@
-        if type == JointType.PRISMATIC:
+        if joint_type_id == JointType.PRISMATIC:
@@
-        if type == JointType.REVOLUTE:
+        if joint_type_id == JointType.REVOLUTE:
@@
-        if type == JointType.BALL:
+        if joint_type_id == JointType.BALL:
@@
-        if type == JointType.FREE or type == JointType.DISTANCE:
+        if joint_type_id == JointType.FREE or joint_type_id == JointType.DISTANCE:
@@
-        if type == JointType.D6:
+        if joint_type_id == JointType.D6:
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.

In `@newton/_src/sim/articulation.py` around lines 202 - 255, The local variable
named type shadows the Python builtin; rename it to joint_type_id (assigned from
joint_type[joint_index]) and update every place that currently checks type ==
JointType.* (PRISMATIC, REVOLUTE, BALL, FREE, DISTANCE, D6) to use joint_type_id
instead; ensure the assignment and all six comparisons in the function that
reference joint_type[joint_index], X_j/v_j construction blocks, and any
subsequent uses are updated to the new name (e.g., replace occurrences in the
blocks handling JointType.PRISMATIC, REVOLUTE, BALL, FREE/DISTANCE, and D6).
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.

Outside diff comments:
In `@newton/_src/sim/articulation.py`:
- Around line 202-255: The local variable named type shadows the Python builtin;
rename it to joint_type_id (assigned from joint_type[joint_index]) and update
every place that currently checks type == JointType.* (PRISMATIC, REVOLUTE,
BALL, FREE, DISTANCE, D6) to use joint_type_id instead; ensure the assignment
and all six comparisons in the function that reference joint_type[joint_index],
X_j/v_j construction blocks, and any subsequent uses are updated to the new name
(e.g., replace occurrences in the blocks handling JointType.PRISMATIC, REVOLUTE,
BALL, FREE/DISTANCE, and D6).

ℹ️ Review info
⚙️ Run configuration

Configuration used: Path: .coderabbit.yml

Review profile: CHILL

Plan: Pro

Run ID: 9471269c-d9bd-496f-aa6e-781c2ed8b666

📥 Commits

Reviewing files that changed from the base of the PR and between 50f943c and d5a3874.

📒 Files selected for processing (1)
  • newton/_src/sim/articulation.py

Rename the local type variable in eval_single_articulation_fk to
joint_type_id so it no longer shadows the Python builtin. This is a
small readability cleanup from review and does not change behavior.
@bobboli bobboli requested a deployment to external-pr-approval April 1, 2026 13:53 — with GitHub Actions Waiting
@bobboli bobboli requested a deployment to external-pr-approval April 1, 2026 13:53 — with GitHub Actions Waiting
@eric-heiden
Copy link
Copy Markdown
Member

Thanks @bobboli, this is a good contribution. We need to figure out how the individual solvers should treat the joint ordering, Featherstone and MuJoCo have their own kinematics functions that are not updated here. Also Kamino may need special treatment, so we will get back to this PR soon.

@preist-nvidia
Copy link
Copy Markdown
Member

Backlog for now, @eric-heiden will follow up. Thanks for the contribution @bobboli!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

Status: Needs Info

Development

Successfully merging this pull request may close these issues.

Make eval_fk independent of joint ordering

3 participants