diff --git a/motrix_env_core/src/motrix_env_core/math/__init__.py b/motrix_env_core/src/motrix_env_core/math/__init__.py
index 88cf693b..cb5e8abf 100644
--- a/motrix_env_core/src/motrix_env_core/math/__init__.py
+++ b/motrix_env_core/src/motrix_env_core/math/__init__.py
@@ -1,6 +1,6 @@
 # Copyright Motphys Technology Co., Ltd. 2025, 2026
 # SPDX-License-Identifier: Apache-2.0
 
-from motrix_env_core.math import quaternion
+from motrix_env_core.math import kinematics, quaternion, rotation
 
-__all__ = ["quaternion"]
+__all__ = ["kinematics", "quaternion", "rotation"]
diff --git a/motrix_env_core/src/motrix_env_core/math/kinematics.py b/motrix_env_core/src/motrix_env_core/math/kinematics.py
new file mode 100644
index 00000000..5a63a6d5
--- /dev/null
+++ b/motrix_env_core/src/motrix_env_core/math/kinematics.py
@@ -0,0 +1,159 @@
+# Copyright Motphys Technology Co., Ltd. 2025, 2026
+# SPDX-License-Identifier: Apache-2.0
+
+"""Differentiable forward kinematics over a serializable kinematic tree.
+
+The tree is pure data (parent links, joint axes, fixed offsets); loading it
+from a simulator model lives in the backend packages. The recursion here is
+plain torch, so gradients flow from body poses back to the root state and
+joint positions — the property world-model imagination relies on: the same
+FK evaluates tracked-body poses for reference motions, logged simulator
+states, and world-model predictions.
+
+Adapted from SuperMimic (supermimic_vibed), Copyright (c) 2026 ETH Zurich,
+Chong Zhang. MIT License.
+"""
+
+from __future__ import annotations
+
+import torch
+
+from motrix_env_core.math.quaternion import to_matrix
+from motrix_env_core.math.rotation import axis_angle_to_mat
+
+__all__ = ["KinematicTree"]
+
+
+class KinematicTree:
+    """Moving-body tree topology for differentiable FK.
+
+    Bodies are listed in tree order (parents before children). Each moving
+    body hosts exactly one joint: either a free/root joint (``joint_idx`` is
+    ``None``, the body takes the root state directly) or a 1-DoF revolute
+    joint (``joint_idx`` indexes into the joint-position vector ``q``).
+    ``off_p`` / ``off_R`` are the fixed pose of the body in its parent frame.
+
+    Args:
+        parent: Parent body index per body; ``-1`` for the root body.
+        joint_idx: q-vector index per body; ``-1`` for the root body.
+        axes: Unit revolute axes in each body's parent frame, ``(N, 3)``.
+        off_p: Body origins in the parent frame, ``(N, 3)``.
+        off_R: Body orientations in the parent frame, as ``xyzw`` quaternions
+            ``(N, 4)`` (the repo-wide convention of :mod:`motrix_env_core.math.quaternion`)
+            or rotation matrices ``(N, 3, 3)``.
+        device: Device for the constant buffers.
+    """
+
+    def __init__(
+        self,
+        parent: list[int],
+        joint_idx: list[int],
+        axes: torch.Tensor,
+        off_p: torch.Tensor,
+        off_R: torch.Tensor,
+        link_names: list[str] | None = None,
+        joint_names: list[str] | None = None,
+        *,
+        device: str | torch.device = "cpu",
+    ):
+        self.parent = list(parent)
+        self.joint_idx = list(joint_idx)
+        self.link_names = list(link_names) if link_names is not None else [str(i) for i in range(len(parent))]
+        self.joint_names = (
+            list(joint_names) if joint_names is not None else [f"joint_{i}" for i in range(max(self.joint_idx) + 1)]
+        )
+        self.axes = torch.as_tensor(axes, dtype=torch.float32, device=device)
+        self.off_p = torch.as_tensor(off_p, dtype=torch.float32, device=device)
+        if off_R.shape[-1] == 4:  # xyzw quaternion (repo-wide convention)
+            self.off_R = to_matrix(torch.as_tensor(off_R, dtype=torch.float32, device=device))
+        else:
+            self.off_R = torch.as_tensor(off_R, dtype=torch.float32, device=device)
+        self.num_bodies = len(self.parent)
+        # Leaf rotations cannot affect any link origin, including all-body output.
+        self._position_rotation_needed = frozenset(par for par in self.parent if par >= 0)
+
+    def positions(
+        self,
+        p: torch.Tensor,
+        R: torch.Tensor,
+        q: torch.Tensor,
+        bodies: list[int] | None = None,
+    ) -> torch.Tensor:
+        """World origins of the requested bodies without computing velocities.
+
+        Gradients flow to the root state ``p``/``R`` and joint positions ``q``.
+
+        Args:
+            p, R: Root position ``(..., 3)`` and rotation ``(..., 3, 3)``.
+            q: Joint positions ``(..., num_joints)``.
+            bodies: Body indices to return; all bodies when ``None``.
+
+        Returns:
+            Body origins stacked on a new second-to-last axis, ``(..., M, 3)``.
+        """
+        positions: list[torch.Tensor | None] = [None] * self.num_bodies
+        rotations: list[torch.Tensor | None] = [None] * self.num_bodies
+        for i in range(self.num_bodies):
+            par = self.parent[i]
+            if par < 0:
+                positions[i], rotations[i] = p, R
+                continue
+            parent_rotation = rotations[par]
+            positions[i] = positions[par] + (parent_rotation @ self.off_p[i].unsqueeze(-1)).squeeze(-1)
+            if i in self._position_rotation_needed:
+                rotations[i] = (parent_rotation @ self.off_R[i]) @ axis_angle_to_mat(
+                    self.axes[i], q[..., self.joint_idx[i]]
+                )
+        idx = range(self.num_bodies) if bodies is None else bodies
+        return torch.stack([positions[i] for i in idx], -2)
+
+    def forward(
+        self,
+        p: torch.Tensor,
+        R: torch.Tensor,
+        v: torch.Tensor,
+        w: torch.Tensor,
+        q: torch.Tensor,
+        qd: torch.Tensor,
+        bodies: list[int] | None = None,
+    ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]:
+        """World poses and velocities of the requested bodies.
+
+        Gradients flow to the root state ``p``/``R``/``v``/``w`` and joint
+        states ``q``/``qd``.
+
+        Args:
+            p, R, v, w: Root position ``(..., 3)``, rotation ``(..., 3, 3)``,
+                and world-frame linear / angular velocity ``(..., 3)``.
+            q, qd: Joint positions / velocities ``(..., num_joints)``.
+            bodies: Body indices to return; all bodies when ``None``.
+
+        Returns:
+            Positions ``(..., M, 3)``, rotations ``(..., M, 3, 3)``, link-origin
+            linear velocities ``(..., M, 3)`` and angular velocities ``(..., M, 3)``.
+        """
+        P: list[torch.Tensor | None] = [None] * self.num_bodies
+        Rs = P.copy()
+        V = P.copy()
+        W = P.copy()
+        for i in range(self.num_bodies):
+            par = self.parent[i]
+            if par < 0:
+                P[i], Rs[i], V[i], W[i] = p, R, v, w
+                continue
+            j = self.joint_idx[i]
+            Rp = Rs[par]
+            pi = P[par] + (Rp @ self.off_p[i].unsqueeze(-1)).squeeze(-1)
+            R_off = Rp @ self.off_R[i]
+            Ri = R_off @ axis_angle_to_mat(self.axes[i], q[..., j])
+            axis_w = (R_off @ self.axes[i].unsqueeze(-1)).squeeze(-1)
+            wi = W[par] + axis_w * qd[..., j : j + 1]
+            vi = V[par] + torch.cross(W[par], pi - P[par], dim=-1)
+            P[i], Rs[i], V[i], W[i] = pi, Ri, vi, wi
+        idx = range(self.num_bodies) if bodies is None else bodies
+        return (
+            torch.stack([P[i] for i in idx], -2),
+            torch.stack([Rs[i] for i in idx], -3),
+            torch.stack([V[i] for i in idx], -2),
+            torch.stack([W[i] for i in idx], -2),
+        )
diff --git a/motrix_env_core/src/motrix_env_core/math/rotation.py b/motrix_env_core/src/motrix_env_core/math/rotation.py
new file mode 100644
index 00000000..642384c4
--- /dev/null
+++ b/motrix_env_core/src/motrix_env_core/math/rotation.py
@@ -0,0 +1,35 @@
+# Copyright Motphys Technology Co., Ltd. 2025, 2026
+# SPDX-License-Identifier: Apache-2.0
+
+"""Torch-only batched rotation primitives (rotation matrices act on column vectors).
+
+Unlike :mod:`motrix_env_core.math.quaternion` (dual NumPy/Torch, ``xyzw``
+quaternions), these helpers are torch-native so they stay differentiable
+inside training graphs.
+"""
+
+from __future__ import annotations
+
+import torch
+
+__all__ = ["axis_angle_to_mat"]
+
+
+def axis_angle_to_mat(axis: torch.Tensor, angle: torch.Tensor) -> torch.Tensor:
+    """Rodrigues for a fixed unit axis ``(..., 3)`` against angle ``(...)``.
+
+    The axis broadcasts against batched angles; the result is a row-major
+    rotation matrix of shape ``broadcast(axis.batch, angle.batch) + (3, 3)``,
+    differentiable in both inputs.
+    """
+    x, y, z = axis.unbind(-1)
+    c, s = torch.cos(angle), torch.sin(angle)
+    C = 1 - c
+    return torch.stack(
+        (
+            c + x * x * C, x * y * C - z * s, x * z * C + y * s,
+            y * x * C + z * s, c + y * y * C, y * z * C - x * s,
+            z * x * C - y * s, z * y * C + x * s, c + z * z * C,
+        ),
+        dim=-1,
+    ).reshape(torch.broadcast_shapes(axis.shape[:-1], angle.shape) + (3, 3))  # fmt: skip
diff --git a/motrix_env_core/src/motrix_env_core/sim/__init__.py b/motrix_env_core/src/motrix_env_core/sim/__init__.py
index c8fe1536..fd8c7163 100644
--- a/motrix_env_core/src/motrix_env_core/sim/__init__.py
+++ b/motrix_env_core/src/motrix_env_core/sim/__init__.py
@@ -16,6 +16,7 @@
     GeomSpec,
     GeomSpecsQuery,
     HeightFieldDataQuery,
+    KinematicTreeQuery,
     LinkCenterOfMassQuery,
     LinkMassQuery,
     ModelQuery,
@@ -99,6 +100,7 @@
     "GeomQuaternionQuery",
     "GeomSpecsQuery",
     "HeightFieldDataQuery",
+    "KinematicTreeQuery",
     "BodyJointPositionQuery",
     "BodyJointVelocityQuery",
     "JointPositionQuery",
diff --git a/motrix_env_core/src/motrix_env_core/sim/model.py b/motrix_env_core/src/motrix_env_core/sim/model.py
index 9ea80d33..b5702642 100644
--- a/motrix_env_core/src/motrix_env_core/sim/model.py
+++ b/motrix_env_core/src/motrix_env_core/sim/model.py
@@ -170,6 +170,24 @@ def compile_with(self, compiler: SimModelCompiler, *, key: str) -> None:
         compiler.compile_body_joint_position_limits(key, self.body)
 
 
+@dataclass(frozen=True)
+class KinematicTreeQuery(ModelQuery):
+    """Differentiable :class:`~motrix_env_core.math.kinematics.KinematicTree` of one body tree.
+
+    ``body`` names the body tree root; the tree's root row takes the
+    floating-base state and ``tree.joint_names`` gives the q-vector order
+    (the backend's own joint order, like every other body-scoped query —
+    resolve joints by name). The resolved value is backend-neutral torch
+    data, so consumers such as MBRL task models never touch a simulator.
+    """
+
+    body: str
+    device: str = "cpu"
+
+    def compile_with(self, compiler: SimModelCompiler, *, key: str) -> None:
+        compiler.compile_kinematic_tree(key, self.body, self.device)
+
+
 @dataclass(frozen=True)
 class DofPositionLimitsQuery(ModelQuery):
     """``(lower, upper)`` float32 arrays in global DOF-position order."""
@@ -330,6 +348,16 @@ def compile_body_joint_position_limits(self, key: str, body: str) -> None:
             body: Name of the body whose joint limits are read.
         """
 
+    @abc.abstractmethod
+    def compile_kinematic_tree(self, key: str, body: str, device: str) -> None:
+        """Compile the differentiable kinematic tree of one body tree.
+
+        Args:
+            key: Logical key under which the result is stored.
+            body: Name of the body tree root whose links are extracted.
+            device: Device for the tree's constant buffers.
+        """
+
     @abc.abstractmethod
     def compile_dof_position_limits(self, key: str) -> None:
         """Compile position limits in canonical DOF-position order.
diff --git a/motrix_env_core/tests/test_math_kinematics.py b/motrix_env_core/tests/test_math_kinematics.py
new file mode 100644
index 00000000..54defdca
--- /dev/null
+++ b/motrix_env_core/tests/test_math_kinematics.py
@@ -0,0 +1,227 @@
+# Copyright Motphys Technology Co., Ltd. 2025, 2026
+# SPDX-License-Identifier: Apache-2.0
+
+"""Analytic-solution and differentiability contracts of the FK engine.
+
+The tree under test is a planar 3-revolute arm (all axes z, unit links along
+x) whose positions, rotations and velocities have closed forms, so the tests
+pin observable behavior without a simulator.
+"""
+
+import math
+
+import pytest
+import torch
+
+from motrix_env_core.math.kinematics import KinematicTree
+from motrix_env_core.math.rotation import axis_angle_to_mat
+
+
+def test_axis_angle_to_mat_rodrigues():
+    axis = torch.tensor([0.0, 0.0, 1.0])
+    angle = torch.tensor([math.pi / 4, -math.pi / 3, 0.0])
+    R = axis_angle_to_mat(axis, angle)
+    c, s = torch.cos(angle), torch.sin(angle)
+    expected = torch.stack(
+        (
+            c,
+            -s,
+            torch.zeros_like(c),
+            s,
+            c,
+            torch.zeros_like(c),
+            torch.zeros_like(c),
+            torch.zeros_like(c),
+            torch.ones_like(c),
+        ),
+        -1,
+    ).reshape(3, 3, 3)
+    torch.testing.assert_close(R, expected)
+    # orthogonal with unit determinant for arbitrary axes
+    torch.manual_seed(7)
+    axes = torch.nn.functional.normalize(torch.randn(5, 3), dim=-1)
+    angles = torch.rand(5) * 2 * math.pi
+    R = axis_angle_to_mat(axes, angles)
+    torch.testing.assert_close(R @ R.transpose(-1, -2), torch.eye(3).expand(5, 3, 3))
+    torch.testing.assert_close(torch.linalg.det(R), torch.ones(5))
+
+
+def planar_arm(device="cpu") -> KinematicTree:
+    """Root body plus two moving links: z-hinge, unit link along x, identity offsets."""
+    n = 3
+    axes = torch.zeros(n, 3, device=device)
+    axes[:, 2] = 1.0
+    off_p = torch.zeros(n, 3, device=device)
+    off_p[1:, 0] = 1.0
+    off_R = torch.eye(3, device=device).repeat(n, 1, 1)
+    return KinematicTree(parent=[-1, 0, 1], joint_idx=[-1, 0, 1], axes=axes, off_p=off_p, off_R=off_R, device=device)
+
+
+def root_pose(theta, device="cpu"):
+    """Root at origin with yaw ``theta`` and unit yaw rate; batch shape (B,)."""
+    theta = torch.as_tensor(theta, dtype=torch.float32, device=device)
+    c, s = torch.cos(theta), torch.sin(theta)
+    zero = torch.zeros_like(theta)
+    one = torch.ones_like(theta)
+    R = torch.stack((c, -s, zero, s, c, zero, zero, zero, one), -1).reshape(theta.shape + (3, 3))
+    p = torch.stack((theta * 0.0, theta * 0.0, theta * 0.0), -1)
+    v = torch.stack((zero, zero, zero), -1)
+    w = torch.stack((zero, zero, torch.ones_like(theta)), -1)
+    return p, R, v, w
+
+
+@pytest.mark.parametrize("device", ["cpu"])
+def test_positions_match_closed_form(device):
+    torch.manual_seed(0)
+    B = 5
+    q = torch.rand(B, 2, device=device) * 2 - 1
+    theta = torch.rand(B, device=device) * 2 * math.pi
+    p, R, _, _ = root_pose(theta, device)
+    pos = planar_arm(device).positions(p, R, q)
+
+    th1 = theta + q[:, 0]
+    # root body origin is p; body-i origin = parent origin + parent rotation @ off_p[i]
+    zeros = torch.zeros_like(theta)
+    expected = torch.stack(
+        (
+            torch.stack((zeros, zeros, zeros), -1),
+            torch.stack((torch.cos(theta), torch.sin(theta), zeros), -1),
+            torch.stack((torch.cos(theta) + torch.cos(th1), torch.sin(theta) + torch.sin(th1), zeros), -1),
+        ),
+        -2,
+    )
+    torch.testing.assert_close(pos, expected)
+
+
+def test_forward_rotations_and_angular_velocity():
+    torch.manual_seed(1)
+    B = 4
+    q = torch.rand(B, 2) * 2 - 1
+    qd = torch.rand(B, 2) * 2 - 1
+    theta = torch.rand(B) * 2 * math.pi
+    p, R, v, w = root_pose(theta)
+    P, Rs, V, W = planar_arm().forward(p, R, v, w, q, qd)
+
+    def yaw_mat(angle):
+        c, s = torch.cos(angle), torch.sin(angle)
+        zero, one = torch.zeros_like(angle), torch.ones_like(angle)
+        return torch.stack((c, -s, zero, s, c, zero, zero, zero, one), -1).reshape(angle.shape + (3, 3))
+
+    assert Rs.shape == (B, 3, 3, 3)
+    torch.testing.assert_close(Rs[:, 0], yaw_mat(theta))
+    torch.testing.assert_close(Rs[:, 1], yaw_mat(theta + q[:, 0]))
+    torch.testing.assert_close(Rs[:, 2], yaw_mat(theta + q[:, 0] + q[:, 1]))
+    # angular velocities compose: root yaw rate + upstream joint rates, all about z
+    expected_w = torch.zeros(B, 3, 3)
+    expected_w[..., 2] = torch.stack((w[:, 2], w[:, 2] + qd[:, 0], w[:, 2] + qd[:, 0] + qd[:, 1]), -1)
+    torch.testing.assert_close(W, expected_w)
+
+
+def test_link_origin_linear_velocity_closed_form():
+    torch.manual_seed(2)
+    B = 3
+    q = torch.rand(B, 2) * 2 - 1
+    qd = torch.rand(B, 2) * 2 - 1
+    theta = torch.rand(B) * 2 * math.pi
+    _, R, _, _ = root_pose(theta)
+    zero3 = torch.zeros(B, 3)
+    w = torch.stack((zero3[:, 0], zero3[:, 1], torch.ones(B)), -1)
+    # root linear velocity v0: pick a nonzero one so cross terms are exercised
+    v0 = torch.tensor([[0.3, -0.2, 0.1]], device=theta.device).repeat(B, 1)
+    P, _, V, _ = planar_arm().forward(zero3, R, v0, w, q, qd)
+
+    th1 = theta + q[:, 0]
+    r1 = torch.stack((torch.cos(theta), torch.sin(theta), torch.zeros_like(theta)), -1)
+    r12 = torch.stack((torch.cos(th1), torch.sin(th1), torch.zeros_like(theta)), -1)
+    w1 = w[:, 2] + qd[:, 0]
+    # V(root)=v0; V(body1)=v0 + ω_root × r1; V(body2) adds ω_1 × (r2 - r1)
+    v1 = v0 + torch.cross(w, r1, dim=-1)
+    v2 = v1 + torch.stack((-w1 * r12[:, 1], w1 * r12[:, 0], torch.zeros_like(w1)), -1)
+    torch.testing.assert_close(V[:, 1], v1)
+    torch.testing.assert_close(V[:, 2], v2)
+
+
+def test_positions_matches_forward():
+    torch.manual_seed(3)
+    q = torch.rand(7, 2)
+    p, R, v, w = root_pose(torch.rand(7) * 2 * math.pi)
+    tree = planar_arm()
+    P, *_ = tree.forward(p, R, v, w, q, torch.zeros_like(q))
+    torch.testing.assert_close(tree.positions(p, R, q), P)
+
+
+def test_gradients_flow_to_q_and_root():
+    tree = planar_arm()
+    q = torch.rand(6, 2, requires_grad=True)
+    theta = (torch.rand(6) * 2 - 1) * math.pi
+    p, R, v, w = root_pose(theta)
+    p = p.detach().requires_grad_(True)
+
+    P, Rs, V, W = tree.forward(p, R, v, w, q, q)
+    loss = (P.square().sum() + W.square().sum()) / P.numel()
+    loss.backward()
+    assert q.grad is not None and q.grad.abs().sum() > 0
+    assert p.grad is not None and p.grad.abs().sum() > 0
+
+    # finite-difference check on the position head in float64
+    tree64 = _double(planar_arm())
+    q64 = q.detach().double().requires_grad_(True)
+    p64 = p.detach().double().requires_grad_(True)
+    R64 = R.double()
+    out = tree64.positions(p64, R64, q64)
+    out.sum().backward()
+    with torch.no_grad():
+        eps = 1e-6
+        for b in range(2):
+            for j in range(2):
+                qp = q64.detach().clone()
+                qp[b, j] += eps
+                qm = q64.detach().clone()
+                qm[b, j] -= eps
+                fd = (tree64.positions(p64.detach(), R64, qp).sum() - tree64.positions(p64.detach(), R64, qm).sum()) / (
+                    2 * eps
+                )
+                torch.testing.assert_close(q64.grad[b, j], fd, rtol=1e-5, atol=1e-5)
+
+
+def test_quaternion_offsets_accepted():
+    # x-axis rotation pins the xyzw convention: (sin, 0, 0, cos), unlike wxyz
+    half = math.pi / 8
+    axes = torch.zeros(2, 3)
+    axes[1, 2] = 1.0
+    quat = torch.tensor([[0.0, 0.0, 0.0, 1.0], [math.sin(half), 0.0, 0.0, math.cos(half)]])
+    c, s = math.cos(2 * half), math.sin(2 * half)
+    off_R_mats = torch.tensor(
+        [
+            [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]],
+            [[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]],
+        ]
+    )
+    tree_q = KinematicTree(
+        parent=[-1, 0],
+        joint_idx=[-1, 0],
+        axes=axes,
+        off_p=torch.tensor([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]]),
+        off_R=quat,
+    )
+    tree_m = KinematicTree(
+        parent=[-1, 0],
+        joint_idx=[-1, 0],
+        axes=axes,
+        off_p=torch.tensor([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]]),
+        off_R=off_R_mats,
+    )
+    p = torch.zeros(1, 3)
+    R = torch.eye(3).unsqueeze(0)
+    q = torch.zeros(1, 1)
+    torch.testing.assert_close(tree_q.off_R, off_R_mats)
+    torch.testing.assert_close(tree_q.positions(p, R, q), tree_m.positions(p, R, q))
+
+
+def _double(tree: KinematicTree) -> KinematicTree:
+    child = KinematicTree.__new__(KinematicTree)
+    child.__dict__.update(tree.__dict__)
+    child.axes = tree.axes.double()
+    child.off_p = tree.off_p.double()
+    child.off_R = tree.off_R.double()
+    return child
diff --git a/motrix_env_core/tests/test_model_query_dispatch.py b/motrix_env_core/tests/test_model_query_dispatch.py
index 2308247b..49eef8db 100644
--- a/motrix_env_core/tests/test_model_query_dispatch.py
+++ b/motrix_env_core/tests/test_model_query_dispatch.py
@@ -12,6 +12,7 @@
     GeomFrictionQuery,
     GeomSpecsQuery,
     HeightFieldDataQuery,
+    KinematicTreeQuery,
     LinkCenterOfMassQuery,
     LinkMassQuery,
     SimModelCompiler,
@@ -70,6 +71,10 @@ def compile_height_field_data(self, key, geom) -> None:
         del geom
         self.dispatched[key] = "heightfield"
 
+    def compile_kinematic_tree(self, key, body, device) -> None:
+        del body, device
+        self.dispatched[key] = "kinematic_tree"
+
 
 def test_model_queries_dispatch_to_typed_compiler_methods() -> None:
     compiler = _DispatchCompiler()
@@ -83,6 +88,7 @@ def test_model_queries_dispatch_to_typed_compiler_methods() -> None:
         "com": LinkCenterOfMassQuery(name="body"),
         "friction": GeomFrictionQuery(name="geom"),
         "heightfield": HeightFieldDataQuery(geom="floor"),
+        "kinematic_tree": KinematicTreeQuery(body="robot", device="cpu"),
     }
 
     model = compiler.compile(SceneCfg(), queries)
diff --git a/motrix_env_motrixsim/src/motrix_env_motrixsim/kinematics.py b/motrix_env_motrixsim/src/motrix_env_motrixsim/kinematics.py
new file mode 100644
index 00000000..a2cbc649
--- /dev/null
+++ b/motrix_env_motrixsim/src/motrix_env_motrixsim/kinematics.py
@@ -0,0 +1,104 @@
+# Copyright Motphys Technology Co., Ltd. 2025, 2026
+# SPDX-License-Identifier: Apache-2.0
+
+"""Build a differentiable :class:`KinematicTree` from a compiled MotrixSim model.
+
+The simulator model is the single source of truth: link topology, fixed
+offsets and joint axes are read from the compiled scene, not re-parsed from
+files. The resulting tree consumes a root state plus a joint-position vector
+ordered as ``tree.joint_names``.
+"""
+
+from __future__ import annotations
+
+import motrixsim as mtx
+import numpy as np
+
+from motrix_env_core.math.kinematics import KinematicTree
+from motrix_env_core.math.quaternion import mul as quat_mul
+from motrix_env_core.math.quaternion import rotate_vector
+
+
+def kinematic_tree(model: mtx.SceneModel, base_link: str, *, device: str = "cpu") -> KinematicTree:
+    """Extract the moving-link tree rooted at ``base_link`` from a compiled model.
+
+    Fixed links are folded into the offsets of their nearest moving
+    descendant; each moving link must carry exactly one 1-DoF joint (hinge).
+    The root row takes the floating-base state directly, so ``base_link``
+    itself must be joint-free.
+
+    Args:
+        model: A compiled scene model.
+        base_link: Name of the robot's base link (usually the body name).
+        device: Device for the tree's constant buffers.
+
+    Returns:
+        A :class:`KinematicTree` whose ``joint_names`` give the q-vector order.
+    """
+    links = [link for link in model.links if link.name == base_link]
+    if not links:
+        raise KeyError(f"Unknown base link {base_link!r}.")
+    root = links[0]
+    if root.num_joints != 0:
+        raise ValueError(f"Base link {base_link!r} carries joints; the root row takes the floating-base state.")
+
+    parent: list[int] = []
+    joint_idx: list[int] = []
+    axes: list[np.ndarray] = []
+    off_p: list[np.ndarray] = []
+    off_q: list[np.ndarray] = []
+    link_names: list[str] = []
+    joint_names: list[str] = []
+
+    def add_moving(link, moving_idx: int, p_offset, q_offset) -> None:
+        pose = link.local_pose  # [x, y, z, i, j, k, w] in the parent link frame
+        p = p_offset + rotate_vector(q_offset, pose[:3])
+        q = quat_mul(q_offset, pose[3:])
+        parent.append(moving_idx)
+        joint_idx.append(len(joint_names))
+        axes.append(np.asarray(link.joints()[0].axis))
+        off_p.append(p)
+        off_q.append(q)
+        joint_names.append(link.joints()[0].name)
+        link_names.append(link.name)
+        # Children are relative to this link's frame; offset accumulation only
+        # spans fixed intermediate links, so it resets at every moving link.
+        recurse(link, len(parent) - 1, np.zeros(3), np.array([0.0, 0.0, 0.0, 1.0]))
+
+    def recurse(link, moving_idx: int, p_offset, q_offset) -> None:
+        for child in link.children:
+            if child.num_joints == 0:
+                pose = child.local_pose
+                recurse(
+                    child,
+                    moving_idx,
+                    p_offset + rotate_vector(q_offset, pose[:3]),
+                    quat_mul(q_offset, pose[3:]),
+                )
+            elif child.num_joints == 1 and child.joints()[0].num_dof_pos == 1:
+                add_moving(child, moving_idx, p_offset, q_offset)
+            else:
+                raise NotImplementedError(
+                    f"Link {child.name!r} needs single 1-DoF hinge joints for FK; found {child.num_joints} joint(s)."
+                )
+
+    # Root row: dummy axis/offset entries, never consumed by the recursion.
+    parent.append(-1)
+    joint_idx.append(-1)
+    axes.append(np.zeros(3))
+    off_p.append(np.zeros(3))
+    off_q.append(np.array([0.0, 0.0, 0.0, 1.0]))
+    link_names.append(root.name)
+    recurse(root, 0, np.zeros(3), np.array([0.0, 0.0, 0.0, 1.0]))
+
+    tree = KinematicTree(
+        parent,
+        joint_idx,
+        np.stack(axes),
+        np.stack(off_p),
+        np.stack(off_q),
+        link_names,
+        joint_names,
+        device=device,
+    )
+    return tree
diff --git a/motrix_env_motrixsim/src/motrix_env_motrixsim/runtime.py b/motrix_env_motrixsim/src/motrix_env_motrixsim/runtime.py
index 07305ede..275c2ed6 100644
--- a/motrix_env_motrixsim/src/motrix_env_motrixsim/runtime.py
+++ b/motrix_env_motrixsim/src/motrix_env_motrixsim/runtime.py
@@ -29,6 +29,7 @@
 )
 from motrix_env_core.sim.read import PhysicsReadProgram, SimDataQuery
 from motrix_env_motrixsim.compiler import MotrixSimSceneCompiler
+from motrix_env_motrixsim.kinematics import kinematic_tree
 from motrix_env_motrixsim.renderer import MotrixSimRenderer
 from motrix_env_motrixsim.sim_data import compile_read_program
 from motrix_env_motrixsim.write_compiler import MotrixSimWriteCompiler
@@ -99,6 +100,12 @@ def compile_body_joint_position_limits(self, key: str, body: str) -> None:
         lower, upper = _body_joint_position_limits(self._model, body)
         self._others[key] = (torch.from_numpy(lower), torch.from_numpy(upper))
 
+    def compile_kinematic_tree(self, key: str, body: str, device: str) -> None:
+        scene_body = self._model.get_body(body)
+        if scene_body is None:
+            raise KeyError(f"Unknown body {body!r}.")
+        self._others[key] = kinematic_tree(self._model, scene_body.base_link.name, device=device)
+
     def compile_dof_position_limits(self, key: str) -> None:
         lower, upper = _dof_position_limits(self._model)
         self._others[key] = (torch.from_numpy(lower), torch.from_numpy(upper))
diff --git a/motrix_env_motrixsim/tests/test_kinematics_loader.py b/motrix_env_motrixsim/tests/test_kinematics_loader.py
new file mode 100644
index 00000000..4f64677e
--- /dev/null
+++ b/motrix_env_motrixsim/tests/test_kinematics_loader.py
@@ -0,0 +1,129 @@
+# Copyright Motphys Technology Co., Ltd. 2025, 2026
+# SPDX-License-Identifier: Apache-2.0
+
+"""Loader parity: the extracted KinematicTree against simulator FK.
+
+Random root states and joint positions are pushed through the backend writes;
+link positions, rotations and velocities sampled from the simulator must match
+the tree's differentiable torch evaluation.
+"""
+
+import numpy as np
+import pytest
+import torch
+
+from motrix_env_core.base import SimCfg
+from motrix_env_core.math.quaternion import to_matrix
+from motrix_env_core.sim import (
+    BodyAngularVelocityWrite,
+    BodyJointPositionWrite,
+    BodyJointVelocityWrite,
+    BodyLinearVelocityWrite,
+    BodyPositionWrite,
+    BodyRotationWrite,
+    LinkAngularVelocityQuery,
+    LinkLinearVelocityQuery,
+    LinkPositionQuery,
+    LinkQuaternionQuery,
+)
+from motrix_env_core.sim.registry import create_sim_backend
+from motrix_env_motrixsim.kinematics import kinematic_tree
+
+
+@pytest.fixture(scope="module")
+def g1():
+    from motrix_env_core import registry
+
+    cfg = registry.make_env_config("g1-wbt-dance", mode="play")
+    backend = create_sim_backend("motrixsim")(cfg.scene, SimCfg(), 1)
+    base_link = cfg.scene.objs.robot.resolved_base_link_name
+    tree = kinematic_tree(backend._model, base_link)
+    return backend, tree, base_link
+
+
+def _euler_to_quat_xyzw(roll, pitch, yaw):
+    cr, sr = np.cos(roll / 2), np.sin(roll / 2)
+    cp, sp = np.cos(pitch / 2), np.sin(pitch / 2)
+    cy, sy = np.cos(yaw / 2), np.sin(yaw / 2)
+    return np.stack(
+        (
+            sr * cp * cy - cr * sp * sy,
+            cr * sp * cy + sr * cp * sy,
+            cr * cp * sy - sr * sp * cy,
+            cr * cp * cy + sr * sp * sy,
+        ),
+        axis=-1,
+    )
+
+
+def test_tree_matches_simulator_fk(g1):
+    backend, tree, base_link = g1
+    rng = np.random.default_rng(0)
+    p = rng.uniform(-1, 1, size=(3,)) + np.array([0.0, 0.0, 1.0])
+    quat = _euler_to_quat_xyzw(*rng.uniform(-np.pi, np.pi, size=(3,)))
+    v = rng.uniform(-1, 1, size=(3,))
+    w = rng.uniform(-1, 1, size=(3,))
+    q = rng.uniform(-0.5, 0.5, size=len(tree.joint_names))
+    qd = rng.uniform(-1, 1, size=len(tree.joint_names))
+
+    program = backend.write_compiler.compile(
+        {
+            "body_position": BodyPositionWrite((base_link,)),
+            "body_rotation": BodyRotationWrite((base_link,)),
+            "body_linear_velocity": BodyLinearVelocityWrite((base_link,)),
+            "body_angular_velocity": BodyAngularVelocityWrite((base_link,)),
+            "joint_position": BodyJointPositionWrite(base_link),
+            "joint_velocity": BodyJointVelocityWrite(base_link),
+        }
+    )
+    reads = {}
+    for name in tree.link_names:
+        reads[f"pos_{name}"] = LinkPositionQuery(link=name)
+        reads[f"rot_{name}"] = LinkQuaternionQuery(link=name)
+        reads[f"linvel_{name}"] = LinkLinearVelocityQuery(link=name)
+        reads[f"angvel_{name}"] = LinkAngularVelocityQuery(link=name)
+    read = backend.compile_reads(reads)
+
+    program.buffer("body_position")[0, 0] = torch.tensor(p)
+    program.buffer("body_rotation")[0, 0] = torch.tensor(quat)
+    program.buffer("body_linear_velocity")[0, 0] = torch.tensor(v)
+    program.buffer("body_angular_velocity")[0, 0] = torch.tensor(w)
+    program.buffer("joint_position")[0, : len(q)] = torch.tensor(q)
+    program.buffer("joint_velocity")[0, : len(qd)] = torch.tensor(qd)
+
+    program.execute(torch.tensor([0], dtype=torch.int64))
+    read.execute(torch.tensor([0], dtype=torch.int64))
+
+    P, R, V, W = tree.forward(
+        torch.tensor(p, dtype=torch.float32).unsqueeze(0),
+        to_matrix(torch.tensor(quat, dtype=torch.float32).unsqueeze(0)),
+        torch.tensor(v, dtype=torch.float32).unsqueeze(0),
+        torch.tensor(w, dtype=torch.float32).unsqueeze(0),
+        torch.tensor(q, dtype=torch.float32).unsqueeze(0),
+        torch.tensor(qd, dtype=torch.float32).unsqueeze(0),
+    )
+    for i, name in enumerate(tree.link_names):
+        np.testing.assert_allclose(read[f"pos_{name}"][0], P[0, i].numpy(), atol=1e-4)
+        np.testing.assert_allclose(read[f"angvel_{name}"][0], W[0, i].numpy(), atol=1e-4)
+        R_sim = to_matrix(torch.tensor(np.asarray(read[f"rot_{name}"][0], dtype=np.float32)))
+        np.testing.assert_allclose(R_sim.numpy(), R[0, i].numpy(), atol=1e-4)
+        np.testing.assert_allclose(read[f"linvel_{name}"][0], V[0, i].numpy(), atol=1e-4)
+
+
+def test_joint_names_cover_every_moving_link(g1):
+    _, tree, _ = g1
+    assert tree.link_names[0]  # root row present
+    assert len(tree.joint_names) == len([j for j in tree.joint_idx if j >= 0])
+    assert len(set(tree.joint_names)) == len(tree.joint_names)
+
+
+def test_kinematic_tree_query_resolves_backend_neutral_tree(g1):
+    backend, _, base_link = g1
+    from motrix_env_core.sim import KinematicTreeQuery
+
+    model = backend.compile_model({"tree": KinematicTreeQuery(body=base_link)})
+    tree = model.others["tree"]
+    assert tree.link_names[0] == base_link
+    assert tree.joint_idx[0] == -1
+    # Same tree the loader produces: parity is already pinned by the test above.
+    assert len(tree.joint_names) == 29