Rigid-body dynamics (real URDF, any robot)

You give this tool the description file of a robot (a URDF file) and it works out the physics of the arm: how heavy it is, how gravity pulls on it and how it moves when pushed.

The control modules on Control treat the robot as a single-integrator plant: they take a joint velocity, they return a joint velocity. That is the level of the driver's API and it is enough for bounding commands, but it is not the actual physics of the arm.

RigidBodyModel is the module that reads the physics. You point it at a URDF file and it gives you the real mass matrix, gravity vector, Coriolis terms, forward dynamics, and the kinematics of any link in the chain. Everything downstream — from gravity compensation to the CBF controllers that need the Jacobian — is built on this.

import jax.numpy as jnp
from dense_armor.dynamics.urdf_dynamics import RigidBodyModel

model = RigidBodyModel("test/fixtures/urdf/panda.urdf")
print(model.n)

9

model.n is the number of real, non-fixed joints the file describes — read from the file, not assumed. For a Franka Panda it is 7 (the arm) or 9 (arm plus the two gripper fingers), depending on which URDF you load. The file used here has the hand, so it is 9.

1. Joint limits

import jax.numpy as jnp
from dense_armor.dynamics.urdf_dynamics import RigidBodyModel

model = RigidBodyModel("test/fixtures/urdf/panda.urdf")
print(model.q_min[:3], model.q_max[:3], model.qd_max[:3])

[-2.9671 -1.8326 -2.9671] [2.9671 1.8326 2.9671] [2.175 2.175 2.175]

Each joint's real position and velocity limits, from the URDF's own <limit> tag, or ±inf wherever the URDF declares none. The passivity + singularity-CBF controller uses these to keep every commanded configuration inside its box; if the URDF does not declare a limit, no row is added to the constraint.

2. The mass matrix

import jax.numpy as jnp
from dense_armor.dynamics.urdf_dynamics import RigidBodyModel

model = RigidBodyModel("test/fixtures/urdf/panda.urdf")
M = model.mass_matrix(jnp.zeros(model.n))
print(M.shape, bool(jnp.allclose(M, M.T)), float(jnp.linalg.eigvalsh(M).min()) > 0)

(9, 9) True True

M(q) is the joint-space mass matrix at configuration q. It is symmetric and positive-definite by construction, built from the link masses and inertia tensors in the file via the standard Lagrangian construction: the kinetic energy is the sum over links of ½ m vᵀv + ½ ωᵀ I ω (each link's mass m and inertia tensor I, with v and ω the velocity of its centre of mass and its angular velocity, both linear in q̇). The jax.grad / jax.jvp machinery does the differentiation; no Christoffel symbols are hand-derived.

Both properties — symmetric and positive-definite — hold without any explicit symmetrization or fix-ups: they come out of the derivation, at every configuration.

3. Gravity and Coriolis terms

import jax.numpy as jnp
from dense_armor.dynamics.urdf_dynamics import RigidBodyModel

model = RigidBodyModel("test/fixtures/urdf/panda.urdf")
q, qd, q_ref = jnp.zeros(model.n), jnp.zeros(model.n), jnp.full(model.n, 0.1)
tau = model.gravity_forces(q) + 50.0 * (q_ref - q) - 10.0 * qd
print(jnp.round(tau[:4], 3))

[5.    2.174 5.    1.483]

gravity_forces(q) is the gravity torque at each joint. bias_forces(q, qd) is the C(q, qd) qd term — Coriolis and centrifugal together, as they always are in the Lagrangian form. The block above builds a gravity-compensating PD: the gravity term plus a proportional term toward a reference plus a derivative term on velocity.

Without gravity compensation, a PD controller on the joint positions needs a large kp to hold the arm against gravity, and that kp produces overshoot when the arm moves. With it, the PD is a small correction on top of a physical baseline.

4. Forward dynamics

import jax.numpy as jnp
from dense_armor.dynamics.urdf_dynamics import RigidBodyModel

model = RigidBodyModel("test/fixtures/urdf/panda.urdf")
q, qd = jnp.zeros(model.n), jnp.zeros(model.n)
qdd = model.forward_dynamics(q, qd, model.gravity_forces(q))
print(float(jnp.abs(qdd).max()))

0.0

forward_dynamics solves M(q) qdd + C(q, qd) qd + g(q) = tau for qdd — the joint acceleration that a torque command tau would produce at this state. Feeding it exactly gravity_forces(q) at zero velocity yields zero acceleration: the model is in equilibrium. With tau = 0 it is the free (torque-free) dynamics, useful for verifying the model: the total energy ½ q̇ᵀ M(q) q̇ + U(q) should be conserved.

With RK4 and tau = 0, the relative energy drift decreases as dt⁴ as the integration step shrinks: 5.9e-7 → 6.0e-11 → 5.5e-15 on the Panda for dt = 10⁻², 10⁻³, 10⁻⁴ s. The convergence order is what confirms the equations are correct; a wrong sign or a transposed inertia tensor would give a much worse ratio.

5. Kinematics of any link

import jax.numpy as jnp
from dense_armor.dynamics.urdf_dynamics import RigidBodyModel

model = RigidBodyModel("test/fixtures/urdf/panda.urdf")
q = jnp.zeros(model.n)
p, J = model.link_position(q, "panda_hand"), model.link_jacobian(q, "panda_hand")
print(jnp.round(p, 3), J.shape)

[ 0.088 -0.     0.926] (3, 9)

Any link name from the URDF works — useful for checking the elbow's position, or for running a CBF on a link that is not the end effector. link_jacobian returns the 3×N position Jacobian.

For a link's full pose (position and orientation) and its 6×N spatial Jacobian — needed by the full 6-DoF controller — use link_pose and link_spatial_jacobian:

import jax.numpy as jnp
from dense_armor.dynamics.urdf_dynamics import RigidBodyModel

model = RigidBodyModel("test/fixtures/urdf/panda.urdf")
q = jnp.zeros(model.n)
p, R = model.link_pose(q, "panda_hand")
print(R.shape, model.link_spatial_jacobian(q, "panda_hand").shape)

(3, 3) (6, 9)

Jspatial has shape (6, N): the top three rows are angular, the bottom three are linear. The order matches Twist = [ω; v], the standard spatial-velocity convention.

6. Xacro files and mimic joints

A .xacro path is expanded automatically. A joint's <mimic> tag (a gripper's two fingers tied together) is respected: model.n counts real independent DOF, not raw <joint> tags. Both are their own pages: xacro support, mimic joints.

API reference

RigidBodyModel

RigidBodyModel(urdf_path)

Real Euler-Lagrange rigid-body dynamics, parsed from a real URDF file.

Parameters

urdf_path : str Path to a real URDF file. Any kinematic tree of revolute, continuous, prismatic, or fixed joints -- not assumed to be a single serial chain.

Attributes

n : int Number of non-fixed joints (degrees of freedom), in a canonical depth-first order from the URDF's root link. q_min, q_max, qd_max : ndarray, shape (n,) Real per-joint position/velocity limits from the URDF's own tags (+/-inf wherever the URDF declares none).

Source code in dense_armor/dynamics/urdf_dynamics.py

def __init__(self, urdf_path):
    links, joints_by_parent, root_link = _parse_urdf(urdf_path)
    self.links = links
    self.root_link = root_link

    self.dof_joints = []
    self.link_parent_joint = {}

    def walk(link_name):
        for j in joints_by_parent.get(link_name, []):
            self.link_parent_joint[j["child"]] = j
            if j["type"] != "fixed" and j["mimic_joint"] is None:
                self.dof_joints.append(j)
            walk(j["child"])

    walk(root_link)
    self.n = len(self.dof_joints)
    self.dof_index = {j["name"]: idx for idx, j in enumerate(self.dof_joints)}

    # A <mimic> joint is not its own independent coordinate: its
    # angle/displacement is master*multiplier+offset, so it maps onto
    # the master's own dof_idx everywhere below, chain-ruled by the
    # multiplier, rather than getting a column of its own.
    self.mimic_map = {}
    for jlist in joints_by_parent.values():
        for j in jlist:
            if j["mimic_joint"] is not None:
                self.mimic_map[j["name"]] = (self.dof_index[j["mimic_joint"]],
                                              j["mimic_multiplier"], j["mimic_offset"])

    self.link_ancestor_dofs = {root_link: []}

    def collect(link_name, ancestors):
        self.link_ancestor_dofs[link_name] = list(ancestors)
        for j in joints_by_parent.get(link_name, []):
            if j["type"] == "fixed":
                next_ancestors = ancestors
            elif j["name"] in self.mimic_map:
                master_idx, mult, _offset = self.mimic_map[j["name"]]
                next_ancestors = ancestors + [(j, master_idx, mult)]
            else:
                next_ancestors = ancestors + [(j, self.dof_index[j["name"]], 1.0)]
            collect(j["child"], next_ancestors)

    collect(root_link, [])

    self.link_names = list(links.keys())

    # Real per-joint limits from the URDF's own <limit> tags (±inf where
    # the URDF declares none, e.g. a "continuous" joint's position, or a
    # joint with no <limit> element at all -- never invented).
    self.q_min = jnp.array([j["q_min"] for j in self.dof_joints])
    self.q_max = jnp.array([j["q_max"] for j in self.dof_joints])
    self.qd_max = jnp.array([j["qd_max"] for j in self.dof_joints])

forward_kinematics

forward_kinematics(q)

Real link poses and joint axes in world frame, as a function of q.

Returns

pos, rot, joint_axis_world : dict Keyed by link name (pos/rot) or joint name (joint_axis_world).

Source code in dense_armor/dynamics/urdf_dynamics.py

def forward_kinematics(self, q):
    """Real link poses and joint axes in world frame, as a function of q.

    Returns
    -------
    pos, rot, joint_axis_world : dict
        Keyed by link name (pos/rot) or joint name (joint_axis_world).
    """
    pos = {self.root_link: jnp.zeros(3)}
    rot = {self.root_link: jnp.eye(3)}
    joint_axis_world = {}

    def walk(link_name, p, r):
        for j in self._children_joints(link_name):
            r_offset = _rpy_to_matrix(jnp.asarray(j["rpy"]))
            p_child = p + r @ jnp.asarray(j["xyz"])
            r_child = r @ r_offset
            axis_world = r_child @ (jnp.asarray(j["axis"]) / jnp.linalg.norm(jnp.asarray(j["axis"])))

            if j["type"] in ("revolute", "continuous"):
                joint_axis_world[j["name"]] = axis_world
                angle = self._dof_value(j, q)
                r_child = r_child @ _axis_angle_matrix(jnp.asarray(j["axis"]), angle)
            elif j["type"] == "prismatic":
                joint_axis_world[j["name"]] = axis_world
                disp = self._dof_value(j, q)
                p_child = p_child + axis_world * disp

            pos[j["child"]] = p_child
            rot[j["child"]] = r_child
            walk(j["child"], p_child, r_child)

    walk(self.root_link, jnp.zeros(3), jnp.eye(3))
    return pos, rot, joint_axis_world

com_positions

com_positions(q)

World-frame center-of-mass position of every link, shape (n_links, 3).

Source code in dense_armor/dynamics/urdf_dynamics.py

def com_positions(self, q):
    """World-frame center-of-mass position of every link, shape (n_links, 3)."""
    pos, rot, _ = self.forward_kinematics(q)
    return jnp.stack([pos[name] + rot[name] @ jnp.asarray(self.links[name]["com"])
                       for name in self.link_names])

mass_matrix

mass_matrix(q)

Real joint-space mass matrix M(q), shape (n, n) -- symmetric positive-definite.

The numeric part is traced once and compiled with jax.jit; the URDF structure is captured as a Python constant during the trace, so the compiled graph has no Python overhead and no eager dispatch.

Source code in dense_armor/dynamics/urdf_dynamics.py

def mass_matrix(self, q):
    """Real joint-space mass matrix M(q), shape (n, n) -- symmetric positive-definite.

    The numeric part is traced once and compiled with ``jax.jit``; the
    URDF structure is captured as a Python constant during the trace, so
    the compiled graph has no Python overhead and no eager dispatch.
    """
    fn = self.__dict__.get("_mass_jit")
    if fn is None:
        fn = jax.jit(self._mass_matrix_impl)
        self.__dict__["_mass_jit"] = fn
    return fn(q)

potential_energy

potential_energy(q)

Real total gravitational potential energy at configuration q.

Source code in dense_armor/dynamics/urdf_dynamics.py

def potential_energy(self, q):
    """Real total gravitational potential energy at configuration q."""
    com = self.com_positions(q)
    masses = jnp.array([self.links[name]["mass"] for name in self.link_names])
    return jnp.sum(masses * com[:, 2]) * _G

kinetic_energy

kinetic_energy(q, qd)

Real total kinetic energy, 0.5qdot^TM(q)*qdot.

Source code in dense_armor/dynamics/urdf_dynamics.py

def kinetic_energy(self, q, qd):
    """Real total kinetic energy, 0.5*qdot^T*M(q)*qdot."""
    m = self.mass_matrix(q)
    return 0.5 * qd @ m @ qd

total_energy

total_energy(q, qd)

Real total mechanical energy (kinetic + potential).

Source code in dense_armor/dynamics/urdf_dynamics.py

def total_energy(self, q, qd):
    """Real total mechanical energy (kinetic + potential)."""
    return self.kinetic_energy(q, qd) + self.potential_energy(q)

gravity_forces

gravity_forces(q)

Real gravity generalized-force vector g(q), shape (n,).

The gradient is traced and compiled with jax.jit once.

Source code in dense_armor/dynamics/urdf_dynamics.py

def gravity_forces(self, q):
    """Real gravity generalized-force vector g(q), shape (n,).

    The gradient is traced and compiled with ``jax.jit`` once.
    """
    fn = self.__dict__.get("_grav_jit")
    if fn is None:
        fn = jax.jit(jax.grad(self.potential_energy))
        self.__dict__["_grav_jit"] = fn
    return fn(q)

bias_forces

bias_forces(q, qd)

Real Coriolis/centrifugal generalized-force vector C(q,qdot)*qdot, shape (n,).

Source code in dense_armor/dynamics/urdf_dynamics.py

def bias_forces(self, q, qd):
    """Real Coriolis/centrifugal generalized-force vector C(q,qdot)*qdot, shape (n,)."""
    mv = lambda qq: self.mass_matrix(qq) @ qd
    mdot_qd = jax.jvp(mv, (q,), (qd,))[1]
    quad = lambda qq: qd @ self.mass_matrix(qq) @ qd
    return mdot_qd - 0.5 * jax.grad(quad)(q)

forward_dynamics

forward_dynamics(q, qd, tau)

Real joint acceleration qddot solving M(q)qddot + C(q,qdot)qdot + g(q) = tau.

Source code in dense_armor/dynamics/urdf_dynamics.py

def forward_dynamics(self, q, qd, tau):
    """Real joint acceleration qddot solving M(q)qddot + C(q,qdot)qdot + g(q) = tau."""
    m = self.mass_matrix(q)
    rhs = tau - self.bias_forces(q, qd) - self.gravity_forces(q)
    return jnp.linalg.solve(m, rhs)

link_position

link_position(q, link_name)

Real world-frame origin position of the named link's own frame.

Source code in dense_armor/dynamics/urdf_dynamics.py

def link_position(self, q, link_name):
    """Real world-frame origin position of the named link's own frame."""
    pos, rot, _ = self.forward_kinematics(q)
    return pos[link_name]

link_jacobian

link_jacobian(q, link_name)

Real translational Jacobian of the named link's own frame origin, shape (3, n).

Source code in dense_armor/dynamics/urdf_dynamics.py

def link_jacobian(self, q, link_name):
    """Real translational Jacobian of the named link's own frame origin, shape (3, n)."""
    pos, rot, joint_axis_world = self.forward_kinematics(q)
    jv = jnp.zeros((3, self.n))
    for j, dof_idx, scale in self.link_ancestor_dofs[link_name]:
        axis_w = joint_axis_world[j["name"]]
        if j["type"] == "prismatic":
            jv = jv.at[:, dof_idx].add(scale * axis_w)
        else:
            p_joint = self._joint_origin_world(j, pos, rot)
            jv = jv.at[:, dof_idx].add(scale * jnp.cross(axis_w, pos[link_name] - p_joint))
    return jv

link_pose

link_pose(q, link_name)

Real (position, rotation matrix) of the named link's own frame, in world frame.

Source code in dense_armor/dynamics/urdf_dynamics.py

def link_pose(self, q, link_name):
    """Real (position, rotation matrix) of the named link's own frame, in world frame."""
    pos, rot, _ = self.forward_kinematics(q)
    return pos[link_name], rot[link_name]

link_spatial_jacobian

link_spatial_jacobian(q, link_name)

Real 6xN spatial Jacobian [angular; linear] of the named link, in world frame.

Source code in dense_armor/dynamics/urdf_dynamics.py

def link_spatial_jacobian(self, q, link_name):
    """Real 6xN spatial Jacobian [angular; linear] of the named link, in world frame."""
    pos, rot, joint_axis_world = self.forward_kinematics(q)
    p_link = pos[link_name]
    jv = jnp.zeros((3, self.n))
    jw = jnp.zeros((3, self.n))
    for j, dof_idx, scale in self.link_ancestor_dofs[link_name]:
        axis_w = joint_axis_world[j["name"]]
        if j["type"] == "prismatic":
            jv = jv.at[:, dof_idx].add(scale * axis_w)
        else:
            p_joint = self._joint_origin_world(j, pos, rot)
            jv = jv.at[:, dof_idx].add(scale * jnp.cross(axis_w, p_link - p_joint))
            jw = jw.at[:, dof_idx].add(scale * axis_w)
    return jnp.concatenate([jw, jv], axis=0)

---

Details

Two-step promotion. Dense-Evolution-Discovery Experiment 61 built the same Euler–Lagrange dynamics — jax.grad / jax.jvp on the kinetic and potential energy, not hand-derived Christoffel symbols — but with every mass, inertia tensor, and joint origin hand-transcribed from one specific Kinova Gen3's URDF. That was the explicit reason it was not promoted at the time: every other module in the package is generic across any joint array, and that one worked for exactly one robot.

Experiment 62 replaced the hardcoded tables with a real parser (xml.etree.ElementTree, no new dependency) and re-validated from scratch.

Validated on three independent real robots, not one:

robot	source	DoF	joint types
Kinova Gen3 7-DoF	the same URDF Kurtz, Wensing & Lin (2021, arXiv:2109.13349) use	7	all revolute
Kinova Gen3 6-DoF	github.com/vincekurtz/kinova_drake — a structurally different chain	6	all revolute
Franka Emika Panda	bulletphysics/bullet3's real pybullet data — a different manufacturer	9	7 revolute + 2 prismatic

Cross-checked against Experiment 61's own hardcoded numbers on the Gen3 7-DoF (mass matrix and gravity forces match to machine precision, 1e-16). On all three: mass matrix symmetric / positive-definite at 20 random configurations, and free dynamics conserve energy with the correct 4th-order RK4 convergence as the integration step shrinks — the Panda's converges tighter (relative drift 5.9e-7 → 6.0e-11 → 5.5e-15) since its published inertia tensors are simpler placeholder values, not a difference in correctness.

Reproducing this: pytest test/test_urdf_dynamics.py.

See also: Passivity + singularity-CBF controller — the task-space controller built on this model. Online dynamics — the module that learns what the URDF does not model (payload, friction, wear).