Coupled joints via <mimic>

A gripper's two fingers usually move together: closing one closes the other. URDF expresses this with a <mimic joint="..." multiplier="..." offset="..."/> tag inside the slaved joint -- its own angle is always multiplier * master_angle + offset, never a free variable. RigidBodyModel previously ignored that tag and gave the slaved joint its own independent coordinate, double-counting a single real degree of freedom.

What changed

A joint with a <mimic> tag no longer gets its own entry in q/qd/tau -- its motion is computed from its master's coordinate wherever the model needs it:

model = RigidBodyModel("panda_arm_hand.urdf.xacro")
model.n                      # 8, not 9 -- the two fingers share one real DOF
model.mimic_map["panda_finger_joint2"]   # (master_dof_idx, multiplier, offset)

Forward kinematics substitutes q[master] * multiplier + offset for the mimic joint's own angle -- exact, not approximate, so gravity/Coriolis terms obtained through jax.grad/jax.jvp on the resulting kinematics are automatically correct. The hand-built geometric Jacobian used for the mass matrix needs its own explicit chain rule: a mimic joint's local Jacobian column (computed from its own axis and origin, same as any joint) is scaled by multiplier and added into its master's column, rather than getting a column of its own.

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

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.