Coupled joints via <mimic>

Some parts of a robot always move together, like the two fingers of a gripper. This page shows how the library understands that one joint simply follows another, so a gripper counts as one moving part and not two.

A gripper's two fingers usually move together: closing one closes the other, at the same rate, in the opposite direction. URDF expresses this with a <mimic> tag inside the slaved joint:

<joint name="panda_finger_joint2" type="prismatic">
  <mimic joint="panda_finger_joint1" multiplier="1.0" offset="0.0"/>
</joint>

The slaved joint's own angle is always multiplier · master_angle + offset. It is not a free variable. The gripper has one degree of freedom, not two.

A parser that ignores the <mimic> tag gives the slaved joint its own independent coordinate. The model then has one extra joint, one extra column in the mass matrix, and one extra torque channel that does not correspond to any real actuator. Every downstream computation — the mass matrix, the gravity vector, the Jacobian, the controllers — is silently wrong.

1. The robot

The Franka Panda with the hand. Two finger joints, one master (panda_finger_joint1), one mimic (panda_finger_joint2).

from dense_armor.dynamics.urdf_dynamics import RigidBodyModel

model = RigidBodyModel("test/fixtures/urdf/xacro_panda/panda_arm_hand.urdf.xacro")
print(model.n, model.mimic_map["panda_finger_joint2"])

8 (7, 1.0, 0.0)

model.n is 8 — 7 arm joints plus 1 independent gripper coordinate, not 9. The two fingers are counted as one real DOF.

2. How mimic joints are represented

The model keeps a map from each mimic joint to (master_dof_idx, multiplier, offset):

(7, 1.0, 0.0)

• 7 — the index of the master joint in the q / qd / tau vectors.
• 1.0 — the multiplier.
• 0.0 — the offset.

Wherever the model needs the mimic joint's own angle (forward kinematics, Jacobian, mass matrix), it computes q[master] · multiplier + offset instead of indexing its own coordinate. The mimic joint contributes no independent column of its own.

3. Why this matters for the mass matrix

Without the mimic handling the model would have 9 coordinates: a 9×9 mass matrix and a ninth torque channel for a finger that no motor drives on its own, and the dynamics could move the two fingers independently, which the real gripper cannot do.

With the mimic handling the model is 8×8: the finger pair is one coordinate, and its mass enters the matrix through the master joint.

4. How mimic joints are handled in the Jacobian

Forward kinematics is exact: q[master] · multiplier + offset is substituted wherever the mimic joint's own angle would appear. Since the rest of the kinematics pipeline uses jax.grad / jax.jvp on this forward-kinematics function, the gravity and Coriolis terms obtained from it are automatically correct — no separate chain rule to write by hand.

The hand-built geometric Jacobian (used inside the mass-matrix construction) does not use autodiff, so it needs its own explicit handling: a mimic joint's local Jacobian column (computed from its own axis and origin, same as any other joint) is scaled by the multiplier and added into its master's column, instead of getting a column of its own.

5. Verifying the handling

Drive the master finger joint by a small amount and check that both fingertips move by the same amount in opposite directions (their local closing axes point opposite ways):

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

model = RigidBodyModel("test/fixtures/urdf/xacro_panda/panda_arm_hand.urdf.xacro")
q = jnp.zeros(model.n)
p1, p2 = (model.link_pose(q, k)[0] for k in ("panda_leftfinger", "panda_rightfinger"))
q1, q2 = (model.link_pose(q.at[7].set(0.02), k)[0] for k in ("panda_leftfinger", "panda_rightfinger"))
print(round(float(jnp.linalg.norm(q1 - p1)), 4), round(float(jnp.linalg.norm(q2 - p2)), 4))

0.02 0.02

Both fingers moved by exactly the master's displacement. In the Jacobian, the mimic column matches the finite-difference derivative of link_pose to under 1e-5.

6. What is not supported

One master per mimic. A mimic joint's joint= attribute must name an independent (non-mimic) joint. Mimicking another mimic joint is not supported.

No transitive chains. Even if the source file names a chain of mimic joints, only the direct link is used. Real published URDFs do not do this; the restriction is a scope decision, not a limitation that matters in practice.

One mimic per master is not required. Multiple mimics can slave off the same master (a three-finger gripper where two fingers follow the first). Each mimic records its own (master_idx, multiplier, offset).

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 67. Checked against a real central finite difference of link_pose, not just plausibility: driving Franka Panda's finger_joint1 (master) by 0.02 moves both fingertips by exactly 0.02 in opposite directions (their local closing axes point opposite ways); the hand-built Jacobian's mimic column matches the finite-difference derivative to under 1e-5.

Only one master per mimic, no transitive chains. A mimic joint's joint= attribute must name an independent (non-mimic) joint; mimicking another mimic joint is not supported — not something real published URDFs do.

Reproducing this: pytest test/test_mimic_joints.py.

See also: Xacro files — the other feature real robot descriptions use that a naive URDF parser misses. The two are independent: a robot can have both, or just one.