Rigid-body dynamics (real URDF, any robot)

Every other module in this package (rate_limiter, cbf_filter, trajectory, kinematic_controller) works at the single-integrator level: you give it a joint velocity, it gives you back a safe joint velocity. RigidBodyModel is different -- it needs an actual physical robot description (a real URDF file) and gives you real torque-level dynamics.

from dense_armor.dynamics.urdf_dynamics import RigidBodyModel

model = RigidBodyModel("panda.urdf")
model.n
model.q_min, model.q_max, model.qd_max

model.n is the number of real, non-fixed joints the file describes -- read from the file, not assumed. model.q_min/model.q_max/model.qd_max are each joint's real position/velocity limit from the URDF's own <limit> tag (+/-inf wherever the URDF declares none), used by the passivity+CBF controller to keep every command inside them. Point RigidBodyModel at any real URDF and it builds the same thing: a mass matrix, a gravity vector, and everything you need to simulate or control that specific robot.

The mass matrix

import jax.numpy as jnp

q = jnp.zeros(model.n)
M = model.mass_matrix(q)

M(q) is the joint-space mass matrix at configuration q -- symmetric, positive-definite, built from the real link masses and inertia tensors in the URDF via the standard Lagrangian construction (kinetic energy from each link's own center-of-mass Jacobian and inertia tensor, summed).

Full dynamics: gravity, Coriolis, and forward simulation

qd = jnp.zeros(model.n)
tau = jnp.zeros(model.n)

g = model.gravity_forces(q)
c_qd = model.bias_forces(q, qd)
qdd = model.forward_dynamics(q, qd, tau)

forward_dynamics solves M(q)*qdd + C(q,qd)*qd + g(q) = tau for qdd -- the real joint acceleration a real torque command tau would produce at this state. gravity_forces and bias_forces are available on their own too, for building a custom controller (a gravity-compensating PD term, for instance).

Kinematics of any link, not only the end effector

p = model.link_position(q, "panda_hand")
J = model.link_jacobian(q, "panda_hand")

Any link name from the URDF works -- useful for checking an elbow's position, not only the end effector. For a link's full pose (position and orientation) and its 6xN spatial Jacobian -- needed by the full 6-DoF controller -- use link_pose and link_spatial_jacobian instead:

p, r = model.link_pose(q, "panda_hand")
Jspatial = model.link_spatial_jacobian(q, "panda_hand")   # 6xN: [angular; linear]

Xacro files and coupled joints

A .xacro path is expanded automatically -- see xacro support. A joint's <mimic> tag (e.g. a gripper's two fingers tied together) is respected too -- see coupled joints via mimic; model.n counts real independent DOF, not raw <joint> tags.

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.

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."""
    pos, rot, joint_axis_world = self.forward_kinematics(q)
    m = jnp.zeros((self.n, self.n))
    for name in self.link_names:
        if self.links[name]["mass"] == 0.0:
            continue
        jv, jw = self._link_jacobian_full(name, pos, rot, joint_axis_world)
        i_local = jnp.asarray(self.links[name]["inertia"])
        i_world = rot[name] @ i_local @ rot[name].T
        m = m + self.links[name]["mass"] * (jv.T @ jv) + jw.T @ i_world @ jw
    return m

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,).

Source code in dense_armor/dynamics/urdf_dynamics.py

def gravity_forces(self, q):
    """Real gravity generalized-force vector g(q), shape (n,)."""
    return jax.grad(self.potential_energy)(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 this same Euler-Lagrange dynamics -- jax.grad/jax.jvp on the kinetic/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 wasn't promoted at the time: every other module here is generic across any joint array, 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 (torque-free) dynamics conserve energy with the correct 4th-order RK4 convergence as the integration step shrinks -- the Panda's converges tighter (rel. drift 5.9e-7 -> 6.0e-11 -> 5.5e-15) since its published inertia tensors are simpler placeholder values, not a difference in correctness.

Mimic joints and xacro files: see their own pages, coupled joints via mimic and xacro support.

Reproducing this: pytest test/test_urdf_dynamics.py.