Loading robots from xacro macros

Robot makers often publish a robot's description as a template file (xacro) that has to be filled in first. This tool reads such a file directly and does the filling in for you.

Real robot descriptions are rarely shipped as a single flat URDF. Manufacturers publish them as .xacro macro files: parametrized building blocks (xacro:macro), math expressions (${-pi/2}), conditionals (xacro:unless) and includes (xacro:include) that get expanded into a plain URDF before anything can parse them. A robot library that only accepts the expanded form forces the user to run the expander by hand and keep two files in sync.

RigidBodyModel accepts either. Point it at a .xacro file and the expansion happens inside the loader.

1. The robot

The Franka Panda from clvrai/furniture. Its description ships as two xacro files (panda_arm.xacro and hand.xacro) plus a top-level panda_arm_hand.urdf.xacro that includes them. Loading the top-level file gives the full arm + hand.

from dense_armor.dynamics.urdf_dynamics import RigidBodyModel

model = RigidBodyModel("test/fixtures/urdf/xacro_panda/panda_arm_hand.urdf.xacro")
print(model.n)

8

model.n is 8 — 7 arm joints plus 1 independent gripper coordinate. The two finger joints are counted as one real DOF because of the <mimic> tag on the second one (see coupled joints via mimic).

2. What happens under the hood

If the path ends in .xacro, the loader calls the real xacro package — xacro.process_file(path).toxml() — and parses the resulting XML. If the path ends in anything else, the loader parses it directly as URDF, exactly as before.

The xacro package is the same expander the ROS ecosystem uses. Nothing about macros, math expressions, conditionals, or includes is reimplemented here: a xacro file is expanded by xacro, and the resulting URDF is parsed by RigidBodyModel.

3. Install

pip install xacro

The xacro package is a small pure-Python dependency. It does not require a ROS installation.

4. Flat URDF files are unchanged

If your URDF is already expanded (no <xacro:...> tags, no .xacro extension), the loader reads it exactly the same as before. There is no cost and no change in behaviour for the common case.

5. Common cases

Parameters. A manufacturer ships the same arm with different payloads, and parametrizes the mass:

<xacro:property name="payload" value="0.5"/>
<link name="link2">
  <inertial>
    <mass value="${payload}"/>
  </inertial>
</link>

xacro substitutes 0.5 for ${payload} before the loader sees the file. The resulting RigidBodyModel reads the substituted value.

Conditionals. A description includes an optional sensor:

<xacro:if value="${with_sensor}">
  <link name="sensor_link">...</link>
</xacro:if>

The with_sensor flag is set at the top of the file (or passed on the command line by the caller's tooling). Expanded, the sensor either exists or does not.

Includes. A description splits the arm and the hand across files:

<xacro:include filename="hand.xacro"/>

The include is resolved relative to the including file. This is what makes the Panda's panda_arm_hand.urdf.xacro work: it includes panda_arm.xacro and hand.xacro in the right order.

6. Debugging an expansion

When the expansion fails — a missing parameter, a macro recursion, an include path the xacro package cannot resolve — the error comes from xacro and the traceback points at the offending line in the macro file. The error is not a RigidBodyModel error and should not be treated as one.

To see the expanded URDF without loading it into the model, call xacro directly:

python -c "import xacro; print(xacro.process_file('panda_arm_hand.urdf.xacro').toxml())" > panda_expanded.urdf

The result is a plain URDF. It can be inspected, diffed against the source, and — if you want — passed to RigidBodyModel as a normal file.

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

Promoted from Dense-Evolution-Discovery Experiment 66. Expanding the Franka Panda's own published macros (panda_arm.xacro + hand.xacro, from clvrai/furniture) first produced a 7-joint model and a KeyError: 'panda_hand': the hand macro attaches with connected_to="panda_link8", but the arm macro's own panda_link8 / panda_joint8 block was commented out in the source. The separately checked-in, pre-expanded panda_arm_hand.urdf in the same upstream repo does include that link, meaning it was generated from an earlier, uncommented version of the same macro. Restoring that block (same real values: mass 0.005, inertia 0.00003, origin 0 0 0.107) reconnects the tree.

New dependency: xacro.

Reproducing this: pytest test/test_xacro_support.py.

See also: Mimic joints — coupled joints (a gripper's two fingers) also need special handling, and the two features are independent: a robot can have both xacro files and mimic joints, or just one of them.